From eeb0cfb3874b1186ebcf4a99ca03ac231de53671 Mon Sep 17 00:00:00 2001 From: klaxalk Date: Tue, 17 Dec 2024 22:35:34 +0000 Subject: [PATCH] deploy: 5bb3c29fbc42fa9f66d699a90150130d86a0cce9 --- .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 | 622 ++ index-sort-l.html | 622 ++ index.html | 622 ++ .../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 | 924 ++ .../impl/publisher_handler.hpp.func.html | 924 ++ .../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 | 284 + .../impl/service_client_handler.hpp.func.html | 284 + ...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 -> 1023 bytes .../subscribe_handler.hpp.func-sort-c.html | 4668 +++++++++ .../impl/subscribe_handler.hpp.func.html | 4668 +++++++++ .../subscribe_handler.hpp.gcov.frameset.html | 19 + .../impl/subscribe_handler.hpp.gcov.html | 446 + .../subscribe_handler.hpp.gcov.overview.html | 111 + .../impl/subscribe_handler.hpp.gcov.png | Bin 0 -> 1519 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 -> 4405 bytes .../publisher_handler.h.func-sort-c.html | 568 ++ .../mrs_lib/publisher_handler.h.func.html | 568 ++ .../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 | 3528 +++++++ .../mrs_lib/subscribe_handler.h.func.html | 3528 +++++++ .../subscribe_handler.h.gcov.frameset.html | 19 + .../mrs_lib/subscribe_handler.h.gcov.html | 565 ++ .../subscribe_handler.h.gcov.overview.html | 141 + .../mrs_lib/subscribe_handler.h.gcov.png | Bin 0 -> 1846 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 -> 454 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 | 353 + .../mrs_lib/timer.h.gcov.overview.html | 88 + mrs_lib/include/mrs_lib/timer.h.gcov.png | Bin 0 -> 927 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 -> 827 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 | 193 + .../timeout_manager.cpp.gcov.overview.html | 48 + .../timeout_manager.cpp.gcov.png | Bin 0 -> 539 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 -> 2003 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 | 168 + .../src/automatic_start.cpp.func.html | 168 + .../automatic_start.cpp.gcov.frameset.html | 19 + .../src/automatic_start.cpp.gcov.html | 1078 ++ .../automatic_start.cpp.gcov.overview.html | 269 + .../src/automatic_start.cpp.gcov.png | Bin 0 -> 3173 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 -> 936 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 | 712 ++ .../tf_source.h.gcov.overview.html | 177 + .../transform_manager/tf_source.h.gcov.png | Bin 0 -> 2245 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 -> 2424 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 | 528 + .../control_manager.cpp.func.html | 528 + .../control_manager.cpp.gcov.frameset.html | 19 + .../control_manager.cpp.gcov.html | 9009 +++++++++++++++++ .../control_manager.cpp.gcov.overview.html | 2252 ++++ .../control_manager.cpp.gcov.png | Bin 0 -> 26131 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 | 180 + .../estimation_manager.cpp.func.html | 180 + .../estimation_manager.cpp.gcov.frameset.html | 19 + .../estimation_manager.cpp.gcov.html | 1412 +++ .../estimation_manager.cpp.gcov.overview.html | 352 + .../estimation_manager.cpp.gcov.png | Bin 0 -> 4637 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 -> 824 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 | 1041 ++ .../transform_manager.cpp.gcov.overview.html | 260 + .../transform_manager.cpp.gcov.png | Bin 0 -> 3411 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 | 2786 +++++ .../src/uav_manager.cpp.gcov.overview.html | 696 ++ mrs_uav_managers/src/uav_manager.cpp.gcov.png | Bin 0 -> 7404 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 | 428 + .../estimators/correction.h.func.html | 428 + .../correction.h.gcov.frameset.html | 19 + .../estimators/correction.h.gcov.html | 2719 +++++ .../correction.h.gcov.overview.html | 679 ++ .../estimators/correction.h.gcov.png | Bin 0 -> 6971 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 | 179 + .../state/passthrough.h.gcov.overview.html | 44 + .../estimators/state/passthrough.h.gcov.png | Bin 0 -> 460 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 | 192 + .../processors/index-detail-sort-l.html | 192 + .../processors/index-detail.html | 192 + .../processors/index-sort-f.html | 152 + .../processors/index-sort-l.html | 152 + .../processors/index.html | 152 + .../geo_mag_declination.h.func-sort-c.html | 96 + .../geo_mag_declination.h.func.html | 96 + .../geo_mag_declination.h.gcov.frameset.html | 19 + .../geo_mag_declination.h.gcov.html | 174 + .../geo_mag_declination.h.gcov.overview.html | 43 + .../geo_mag_declination.h.gcov.png | Bin 0 -> 494 bytes .../mag_declination/index-detail-sort-f.html | 102 + .../mag_declination/index-detail-sort-l.html | 102 + .../mag_declination/index-detail.html | 102 + .../mag_declination/index-sort-f.html | 102 + .../mag_declination/index-sort-l.html | 102 + .../processors/mag_declination/index.html | 102 + .../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 -> 661 bytes .../proc_mag_declination.h.func-sort-c.html | 112 + .../proc_mag_declination.h.func.html | 112 + .../proc_mag_declination.h.gcov.frameset.html | 19 + .../proc_mag_declination.h.gcov.html | 231 + .../proc_mag_declination.h.gcov.overview.html | 57 + .../proc_mag_declination.h.gcov.png | Bin 0 -> 677 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 | 353 + .../agl/garmin_agl.cpp.gcov.overview.html | 88 + .../estimators/agl/garmin_agl.cpp.gcov.png | Bin 0 -> 1143 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 | 840 ++ .../alt_generic.cpp.gcov.overview.html | 209 + .../altitude/alt_generic.cpp.gcov.png | Bin 0 -> 2923 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 | 796 ++ .../hdg_generic.cpp.gcov.overview.html | 198 + .../heading/hdg_generic.cpp.gcov.png | Bin 0 -> 2312 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 | 393 + .../hdg_passthrough.cpp.gcov.overview.html | 98 + .../heading/hdg_passthrough.cpp.gcov.png | Bin 0 -> 1239 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 | 888 ++ .../lat_generic.cpp.gcov.overview.html | 221 + .../lateral/lat_generic.cpp.gcov.png | Bin 0 -> 3153 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 | 120 + .../state/passthrough.cpp.func.html | 120 + .../state/passthrough.cpp.gcov.frameset.html | 19 + .../state/passthrough.cpp.gcov.html | 452 + .../state/passthrough.cpp.gcov.overview.html | 112 + .../estimators/state/passthrough.cpp.gcov.png | Bin 0 -> 1608 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 | 638 ++ .../state_generic.cpp.gcov.overview.html | 159 + .../state/state_generic.cpp.gcov.png | Bin 0 -> 1972 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 | 1659 +++ .../landoff_tracker.cpp.gcov.overview.html | 414 + .../landoff_tracker.cpp.gcov.png | Bin 0 -> 4860 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 | 1364 +++ .../line_tracker.cpp.gcov.overview.html | 340 + .../line_tracker/line_tracker.cpp.gcov.png | Bin 0 -> 3795 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 | 3993 ++++++++ .../mpc_tracker.cpp.gcov.overview.html | 998 ++ .../src/mpc_tracker/mpc_tracker.cpp.gcov.png | Bin 0 -> 12562 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 | 1259 +++ .../speed_tracker.cpp.gcov.overview.html | 314 + .../speed_tracker/speed_tracker.cpp.gcov.png | Bin 0 -> 3278 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 | 2514 +++++ ...ajectory_generation.cpp.gcov.overview.html | 628 ++ .../mrs_trajectory_generation.cpp.gcov.png | Bin 0 -> 7482 bytes ruby.png | Bin 0 -> 141 bytes snow.png | Bin 0 -> 141 bytes updown.png | Bin 0 -> 117 bytes 1038 files changed, 184794 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/mag_declination/geo_mag_declination.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/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_mag_declination.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_mag_declination.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_mag_declination.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_mag_declination.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_mag_declination.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_mag_declination.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 coverage59.8%59.8% \ 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..33f653d359 --- /dev/null +++ b/index-sort-f.html @@ -0,0 +1,622 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top levelHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:143932212965.0 %
Date:2024-12-17 22:35:29Functions:2654443759.8 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Directory Sort by nameLine Coverage Sort by line coverageFunctions Sort by function coverage
mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination +
0.0%
+ 		0.0 %0 / 260.0 %0 / 4
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.5%18.5%
+ 		18.5 %91 / 49121.1 %12 / 57
mrs_uav_trackers/src/speed_tracker +
14.3%14.3%
+ 		14.3 %64 / 44836.8 %7 / 19
mrs_uav_trackers/src/joy_tracker +
46.5%46.5%
+ 		46.5 %66 / 14238.9 %7 / 18
mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors +
56.4%56.4%
+ 		56.4 %133 / 23640.5 %17 / 42
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_lib/include/mrs_lib/impl +
78.5%78.5%
+ 		78.5 %366 / 46655.4 %835 / 1506
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 +
79.0%79.0%
+ 		79.0 %781 / 98857.8 %853 / 1475
mrs_uav_managers/include/mrs_uav_managers +
100.0%
+ 		100.0 %12 / 1260.0 %6 / 10
mrs_uav_state_estimators/src/estimators/agl +
66.7%66.7%
+ 		66.7 %82 / 12360.0 %6 / 10
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators +
37.0%37.0%
+ 		37.0 %416 / 112560.4 %67 / 111
mrs_uav_trackers/src/line_tracker +
68.1%68.1%
+ 		68.1 %325 / 47763.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.8%53.8%
+ 		53.8 %213 / 39666.7 %22 / 33
mrs_uav_trackers/src/landoff_tracker +
73.4%73.4%
+ 		73.4 %441 / 60167.7 %21 / 31
mrs_uav_managers/src/control_manager/common +
41.3%41.3%
+ 		41.3 %234 / 56767.7 %21 / 31
mrs_uav_managers/include/transform_manager +
48.5%48.5%
+ 		48.5 %195 / 40268.4 %13 / 19
mrs_uav_managers/include/mrs_uav_managers/control_manager +
55.2%55.2%
+ 		55.2 %53 / 9670.4 %19 / 27
mrs_uav_state_estimators/src/estimators/state +
64.6%64.6%
+ 		64.6 %326 / 50573.7 %28 / 38
mrs_uav_state_estimators/src/estimators/lateral +
55.0%55.0%
+ 		55.0 %241 / 43875.8 %25 / 33
mrs_lib/src/transformer +
66.1%66.1%
+ 		66.1 %185 / 28076.9 %20 / 26
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/state +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
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/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 +
65.8%65.8%
+ 		65.8 %438 / 66683.3 %40 / 48
mrs_uav_managers/src/control_manager +
69.2%69.2%
+ 		69.2 %2592 / 374586.3 %107 / 124
mrs_uav_managers/src +
72.4%72.4%
+ 		72.4 %1252 / 172987.7 %64 / 73
mrs_uav_controllers/src +
80.5%80.5%
+ 		80.5 %1751 / 217487.9 %58 / 66
mrs_lib/src/safety_zone +
63.3%63.3%
+ 		63.3 %31 / 4988.9 %8 / 9
mrs_lib/src/profiler +
36.6%36.6%
+ 		36.6 %37 / 10190.0 %9 / 10
mrs_uav_trackers/src/mpc_tracker +
82.9%82.9%
+ 		82.9 %1395 / 168290.0 %45 / 50
mrs_uav_managers/src/transform_manager +
81.4%81.4%
+ 		81.4 %351 / 43192.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 +
87.5%87.5%
+ 		87.5 %293 / 335100.0 %22 / 22
mrs_uav_trajectory_generation/src +
67.5%67.5%
+ 		67.5 %755 / 1118100.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..5d685ca6de --- /dev/null +++ b/index-sort-l.html @@ -0,0 +1,622 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top levelHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:143932212965.0 %
Date:2024-12-17 22:35:29Functions:2654443759.8 %
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_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination +
0.0%
+ 		0.0 %0 / 260.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.3%14.3%
+ 		14.3 %64 / 44836.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.5%18.5%
+ 		18.5 %91 / 49121.1 %12 / 57
mrs_lib/src/profiler +
36.6%36.6%
+ 		36.6 %37 / 10190.0 %9 / 10
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators +
37.0%37.0%
+ 		37.0 %416 / 112560.4 %67 / 111
mrs_lib/src/safety_zone/polygon +
40.8%40.8%
+ 		40.8 %42 / 10350.0 %4 / 8
mrs_uav_managers/src/control_manager/common +
41.3%41.3%
+ 		41.3 %234 / 56767.7 %21 / 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 %195 / 40268.4 %13 / 19
mrs_lib/src/transform_broadcaster +
53.3%53.3%
+ 		53.3 %16 / 3066.7 %2 / 3
mrs_uav_state_estimators/src/estimators/altitude +
53.8%53.8%
+ 		53.8 %213 / 39666.7 %22 / 33
mrs_uav_state_estimators/src/estimators/lateral +
55.0%55.0%
+ 		55.0 %241 / 43875.8 %25 / 33
mrs_uav_managers/include/mrs_uav_managers/control_manager +
55.2%55.2%
+ 		55.2 %53 / 9670.4 %19 / 27
mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors +
56.4%56.4%
+ 		56.4 %133 / 23640.5 %17 / 42
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/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_state_estimators/src/estimators/state +
64.6%64.6%
+ 		64.6 %326 / 50573.7 %28 / 38
mrs_uav_managers/src/estimation_manager +
65.8%65.8%
+ 		65.8 %438 / 66683.3 %40 / 48
mrs_lib/src/transformer +
66.1%66.1%
+ 		66.1 %185 / 28076.9 %20 / 26
mrs_uav_state_estimators/src/estimators/agl +
66.7%66.7%
+ 		66.7 %82 / 12360.0 %6 / 10
mrs_uav_trajectory_generation/src +
67.5%67.5%
+ 		67.5 %755 / 1118100.0 %22 / 22
mrs_uav_trackers/src/line_tracker +
68.1%68.1%
+ 		68.1 %325 / 47763.3 %19 / 30
mrs_uav_managers/src/control_manager +
69.2%69.2%
+ 		69.2 %2592 / 374586.3 %107 / 124
mrs_lib/include/mrs_lib/geometry +
71.0%71.0%
+ 		71.0 %66 / 9342.5 %51 / 120
mrs_uav_managers/src +
72.4%72.4%
+ 		72.4 %1252 / 172987.7 %64 / 73
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.4%73.4%
+ 		73.4 %441 / 60167.7 %21 / 31
mrs_lib/include/mrs_lib/impl +
78.5%78.5%
+ 		78.5 %366 / 46655.4 %835 / 1506
mrs_lib/src/timeout_manager +
79.0%79.0%
+ 		79.0 %49 / 6281.8 %9 / 11
mrs_lib/include/mrs_lib +
79.0%79.0%
+ 		79.0 %781 / 98857.8 %853 / 1475
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 %351 / 43192.9 %13 / 14
mrs_uav_trackers/src/mpc_tracker +
82.9%82.9%
+ 		82.9 %1395 / 168290.0 %45 / 50
mrs_lib/src/timer +
84.0%84.0%
+ 		84.0 %105 / 12583.3 %15 / 18
mrs_uav_autostart/src +
87.5%87.5%
+ 		87.5 %293 / 335100.0 %22 / 22
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_state_estimators/include/mrs_uav_state_estimators/estimators/agl +
100.0%
+ 		100.0 %4 / 4100.0 %3 / 3
mrs_uav_managers/include/control_manager +
100.0%
+ 		100.0 %4 / 4100.0 %10 / 10
mrs_lib/src/utils +
100.0%
+ 		100.0 %7 / 7100.0 %2 / 2
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/altitude +
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..778855291f --- /dev/null +++ b/index.html @@ -0,0 +1,622 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top levelHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:143932212965.0 %
Date:2024-12-17 22:35:29Functions:2654443759.8 %
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.0%79.0%
+ 		79.0 %781 / 98857.8 %853 / 1475
mrs_lib/include/mrs_lib/geometry +
71.0%71.0%
+ 		71.0 %66 / 9342.5 %51 / 120
mrs_lib/include/mrs_lib/impl +
78.5%78.5%
+ 		78.5 %366 / 46655.4 %835 / 1506
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 +
36.6%36.6%
+ 		36.6 %37 / 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 +
87.5%87.5%
+ 		87.5 %293 / 335100.0 %22 / 22
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 +
55.2%55.2%
+ 		55.2 %53 / 9670.4 %19 / 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 %195 / 40268.4 %13 / 19
mrs_uav_managers/src +
72.4%72.4%
+ 		72.4 %1252 / 172987.7 %64 / 73
mrs_uav_managers/src/control_manager +
69.2%69.2%
+ 		69.2 %2592 / 374586.3 %107 / 124
mrs_uav_managers/src/control_manager/common +
41.3%41.3%
+ 		41.3 %234 / 56767.7 %21 / 31
mrs_uav_managers/src/estimation_manager +
65.8%65.8%
+ 		65.8 %438 / 66683.3 %40 / 48
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 %351 / 43192.9 %13 / 14
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators +
37.0%37.0%
+ 		37.0 %416 / 112560.4 %67 / 111
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 +
56.4%56.4%
+ 		56.4 %133 / 23640.5 %17 / 42
mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination +
0.0%
+ 		0.0 %0 / 260.0 %0 / 4
mrs_uav_state_estimators/src/estimators/agl +
66.7%66.7%
+ 		66.7 %82 / 12360.0 %6 / 10
mrs_uav_state_estimators/src/estimators/altitude +
53.8%53.8%
+ 		53.8 %213 / 39666.7 %22 / 33
mrs_uav_state_estimators/src/estimators/heading +
18.5%18.5%
+ 		18.5 %91 / 49121.1 %12 / 57
mrs_uav_state_estimators/src/estimators/lateral +
55.0%55.0%
+ 		55.0 %241 / 43875.8 %25 / 33
mrs_uav_state_estimators/src/estimators/state +
64.6%64.6%
+ 		64.6 %326 / 50573.7 %28 / 38
mrs_uav_trackers/src/joy_tracker +
46.5%46.5%
+ 		46.5 %66 / 14238.9 %7 / 18
mrs_uav_trackers/src/landoff_tracker +
73.4%73.4%
+ 		73.4 %441 / 60167.7 %21 / 31
mrs_uav_trackers/src/line_tracker +
68.1%68.1%
+ 		68.1 %325 / 47763.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.9%82.9%
+ 		82.9 %1395 / 168290.0 %45 / 50
mrs_uav_trackers/src/speed_tracker +
14.3%14.3%
+ 		14.3 %64 / 44836.8 %7 / 19
mrs_uav_trajectory_generation/src +
67.5%67.5%
+ 		67.5 %755 / 1118100.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..703e5fcf72 --- /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-12-17 22:35:29Functions: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_Li0EEEIdEEv105242
mrs_lib::Vector3Converter::Vector3Converter(tf2::Vector3 const&)325420
mrs_lib::AttitudeConverter::AttitudeConverter<double>(Eigen::AngleAxis<double>)663488
+
+
+ + + +
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..4f9b810bd9 --- /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-12-17 22:35:29Functions: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&)325420
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>)663488
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_Li0EEEIdEEv105242
_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..580a075b81 --- /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-12-17 22:35:29Functions: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      325420 :   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    15407611 :   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      663488 :   AttitudeConverter(const Eigen::AngleAxis<T> angle_axis) {
+     254      660304 :     double       angle = angle_axis.angle();
+     255      660677 :     tf2::Vector3 axis(angle_axis.axis()[0], angle_axis.axis()[1], angle_axis.axis()[2]);
+     256             : 
+     257      661788 :     tf2_quaternion_.setRotation(axis, angle);
+     258      661937 :   }
+     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      105242 :   operator Eigen::Quaternion<T>() const {
+     334             : 
+     335      105242 :     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-12-17 22:35:29Functions: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&)116
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::print_changed_params(mrs_uav_state_estimators::LateralEstimatorConfig const&)116
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::LateralEstimatorConfig&, unsigned int)116
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&)116
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::load_defaults(mrs_uav_state_estimators::AltitudeEstimatorConfig&)196
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::print_changed_params(mrs_uav_state_estimators::AltitudeEstimatorConfig const&)196
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)196
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&)196
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::update_config(mrs_uav_state_estimators::LateralEstimatorConfig const&)232
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&)348
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&)348
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_cast<double>(boost::any&, double*&)348
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_cast<int>(boost::any&, int*&)348
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::update_config(mrs_uav_state_estimators::AltitudeEstimatorConfig const&)392
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&)402
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&)402
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::load_defaults(mrs_uav_state_estimators::CorrectionConfig&)402
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::print_changed_params(mrs_uav_state_estimators::CorrectionConfig const&)402
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::CorrectionConfig&, unsigned int)402
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_cast<double>(boost::any&, double*&)402
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_cast<int>(boost::any&, int*&)402
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::DynamicReconfigureMgr(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)402
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&)402
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&)588
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&)588
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_cast<double>(boost::any&, double*&)588
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_cast<int>(boost::any&, int*&)588
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::update_config(mrs_uav_state_estimators::CorrectionConfig const&)804
+
+
+ + + +
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..d4c770f4a3 --- /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-12-17 22:35:29Functions: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&)402
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&)402
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&)402
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::update_config(mrs_uav_state_estimators::CorrectionConfig const&)804
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::print_changed_params(mrs_uav_state_estimators::CorrectionConfig const&)402
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::CorrectionConfig&, unsigned int)402
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*&)402
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_cast<int>(boost::any&, int*&)402
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::DynamicReconfigureMgr(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)402
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&)402
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&)348
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&)348
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&)116
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::update_config(mrs_uav_state_estimators::LateralEstimatorConfig const&)232
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::print_changed_params(mrs_uav_state_estimators::LateralEstimatorConfig const&)116
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::LateralEstimatorConfig&, unsigned int)116
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*&)348
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_cast<int>(boost::any&, int*&)348
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&)116
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&)588
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&)588
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&)196
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::update_config(mrs_uav_state_estimators::AltitudeEstimatorConfig const&)392
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::print_changed_params(mrs_uav_state_estimators::AltitudeEstimatorConfig const&)196
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)196
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*&)588
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_cast<int>(boost::any&, int*&)588
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&)196
+
+
+ + + +
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..e12559f827 --- /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-12-17 22:35:29Functions: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         714 :   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         714 :         m_server(m_server_mtx, nh),
+      54             :         m_usr_cbf(user_callback),
+      55         714 :         m_pl(nh, print_values, node_name)
+      56             :   {
+      57             :     // initialize the dynamic reconfigure callback
+      58         714 :     m_server.setCallback(boost::bind(&DynamicReconfigureMgr<ConfigType>::dynamic_reconfigure_callback, this, _1, _2));
+      59         714 :   };
+      60             : 
+      61             :   /* Constructor overloads //{ */
+      62             :   // Convenience constructor to enable writing DynamicReconfigureMgr<MyConfig> drmgr(nh, node_name)
+      63         402 :   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        1428 :   void update_config(const ConfigType& cfg)
+      72             :   {
+      73        1428 :     m_server.updateConfig(cfg);
+      74        1428 :   }
+      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         714 :   void dynamic_reconfigure_callback(ConfigType& new_config, uint32_t level)
+     107             :   {
+     108         714 :     if (m_print_values)
+     109             :     {
+     110         714 :       if (m_node_name.empty())
+     111           0 :         ROS_INFO("Dynamic reconfigure request received");
+     112             :       else
+     113         714 :         ROS_INFO("[%s]: Dynamic reconfigure request received", m_node_name.c_str());
+     114             :     }
+     115             : 
+     116         714 :     if (m_not_initialized)
+     117             :     {
+     118         714 :       load_defaults(new_config);
+     119         714 :       update_config(new_config);
+     120             :     }
+     121         714 :     if (m_print_values)
+     122             :     {
+     123         714 :       print_changed_params(new_config);
+     124             :     }
+     125         714 :     m_not_initialized = false;
+     126         714 :     config = new_config;
+     127         714 :     if (m_usr_cbf)
+     128         312 :       m_usr_cbf(new_config, level);
+     129         714 :   }
+     130             : 
+     131             :   template <typename T>
+     132        1338 :   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        2676 :     boost::shared_ptr<const param_descr_t> cast_descr = boost::dynamic_pointer_cast<const param_descr_t>(descr);
+     136        1338 :     m_pl.loadParam(name, config.*(cast_descr->field));
+     137        1338 :   }
+     138             :   
+     139         714 :   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        1428 :     std::vector<typename ConfigType::AbstractParamDescriptionConstPtr> descrs = new_config.__getParamDescriptions__();
+     143        2052 :     for (auto& descr : descrs)
+     144             :     {
+     145        2676 :       std::string name = descr->name;
+     146        1338 :       size_t pos = name.find("__");
+     147        1338 :       while (pos != name.npos)
+     148             :       {
+     149           0 :         name.replace(pos, 2, "/");
+     150           0 :         pos = name.find("__");
+     151             :       }
+     152             : 
+     153        1338 :       if (descr->type == "bool")
+     154           0 :         load_param<bool>(name, descr, new_config);
+     155        1338 :       else if (descr->type == "int")
+     156           0 :         load_param<int>(name, descr, new_config);
+     157        1338 :       else if (descr->type == "double")
+     158        1338 :         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         714 :   }
+     168             : 
+     169             :   // method for printing names and values of new received parameters (prints only the changed ones) //{
+     170         714 :   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        1428 :     std::vector<typename ConfigType::AbstractParamDescriptionConstPtr> descrs = new_config.__getParamDescriptions__();
+     174        2052 :     for (auto& descr : descrs)
+     175             :     {
+     176           0 :       boost::any val, old_val;
+     177        1338 :       descr->getValue(new_config, val);
+     178        1338 :       descr->getValue(config, old_val);
+     179        1338 :       std::string name = descr->name;
+     180        1338 :       const size_t pos = name.find("__");
+     181        1338 :       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        1338 :       if (try_cast(val, intval))
+     199             :       {
+     200           0 :         if (m_not_initialized || !try_compare(old_val, intval))
+     201           0 :           print_value(name, *intval);
+     202        1338 :       } else if (try_cast(val, doubleval))
+     203             :       {
+     204        1338 :         if (m_not_initialized || !try_compare(old_val, doubleval))
+     205        1338 :           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         714 :   }
+     220             :   //}
+     221             :   
+     222             :   // helper method for parameter printing
+     223             :   template <typename T>
+     224        1338 :   inline void print_value(const std::string& name, const T& val)
+     225             :   {
+     226        1338 :     if (m_node_name.empty())
+     227           0 :       std::cout << "\t" << name << ":\t" << val << std::endl;
+     228             :     else
+     229        1338 :       ROS_INFO_STREAM("[" << m_node_name << "]: parameter '" << name << "':\t" << val);
+     230        1338 :   }
+     231             :   // helper methods for automatic parameter value parsing
+     232             :   template <typename T>
+     233        2676 :   inline bool try_cast(boost::any& val, T*& out)
+     234             :   {
+     235        2676 :     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..ca87482be4 --- /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-12-17 22:35:29Functions: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)1014
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interpUnwrapped(double, double, double)1014
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interp(double, double, double)1014
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)10390
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>)470462
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::diff(double, double)537347
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)674240
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interpUnwrapped(double, double, double)674240
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interp(double, double, double)674240
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::radians const&)919279
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)962369
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>)1018771
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)1368508
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(double)2067256
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::wrap(double)2089813
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::unwrap(double, double)3389417
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::diff(double, double)3403399
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>)4087867
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(double)8275729
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::wrap(double)9081331
+
+
+ + + +
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..4ead255f35 --- /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-12-17 22:35:29Functions: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)1014
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interpUnwrapped(double, double, double)1014
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>)1018771
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::diff(double, double)537347
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>)470462
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::dist(double, double)10390
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::wrap(double)2089813
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)1014
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&)919279
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)962369
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(double)2067256
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)674240
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interpUnwrapped(double, double, double)674240
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>)4087867
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::diff(double, double)3403399
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)9081331
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)674240
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::unwrap(double, double)3389417
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&)1368508
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(double)8275729
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..59ce6a74db --- /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-12-17 22:35:29Functions: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    10382990 :       cyclic(const flt val) : val(wrap(val)){};
+      62             :       /*!
+      63             :        * \brief Copy constructor.
+      64             :        *
+      65             :        * \param val initialization value.
+      66             :        */
+      67     2330877 :       cyclic(const cyclic& other) : val(other.val){};
+      68             :       /*!
+      69             :        * \brief Copy constructor.
+      70             :        *
+      71             :        * \param val initialization value.
+      72             :        */
+      73      919279 :       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    11231149 :       static flt wrap(const flt val)
+     115             :       {
+     116             :         // these few ifs should cover most cases, improving speed and precision
+     117    11231149 :         if (val >= minimum)
+     118             :         {
+     119    10442906 :           if (val < supremum)  // value is actually in range and doesn't need to be wrapped
+     120    10215754 :             return val;
+     121      227166 :           else if (val < supremum + range)
+     122       63843 :             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      788237 :           if (val >= minimum - range)
+     126      624909 :             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      326659 :         const flt rem = std::fmod(val - minimum, range);
+     131      326659 :         const flt wrapped = rem + minimum + std::signbit(rem) * range;
+     132      326490 :         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     3419418 :       static flt unwrap(const flt what, const flt from)
+     158             :       {
+     159     3419418 :         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     3960746 :       static flt diff(const flt minuend, const flt subtrahend)
+     194             :       {
+     195     3960746 :         return diff(cyclic(minuend), cyclic(subtrahend));
+     196             :       }
+     197             : 
+     198     5126638 :       static flt diff(const cyclic minuend, const cyclic subtrahend)
+     199             :       {
+     200     5126638 :         const flt d = minuend.val - subtrahend.val;
+     201     5126638 :         if (d < -half_range)
+     202       43767 :           return d + range;
+     203     5082871 :         if (d >= half_range)
+     204       26683 :           return d - range;
+     205     5056186 :         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       20392 :       static flt dist(const flt from, const flt to)
+     220             :       {
+     221       20392 :         return dist(cyclic(from), cyclic(to));
+     222             :       }
+     223             : 
+     224      480472 :       static flt dist(const cyclic from, const cyclic to)
+     225             :       {
+     226      480472 :         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      675254 :       static flt interpUnwrapped(const flt from, const flt to, const flt coeff)
+     243             :       {
+     244      675254 :         return interpUnwrapped(cyclic(from), cyclic(to), coeff);
+     245             :       }
+     246             : 
+     247      675254 :       static flt interpUnwrapped(const cyclic from, const cyclic to, const flt coeff)
+     248             :       {
+     249      675254 :         const flt dang = diff(to, from);
+     250      675254 :         const flt intp = from.val + coeff * dang;
+     251      675254 :         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      675254 :       static flt interp(const flt from, const flt to, const flt coeff)
+     265             :       {
+     266      675254 :         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-12-17 22:35:29Functions: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..1ca0e02765 --- /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-12-17 22:35:29Functions: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..1d44096cf3 --- /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-12-17 22:35:29Functions: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..879bb5d863 --- /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-12-17 22:35:29Functions: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..04308eb868 --- /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-12-17 22:35:29Functions: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..ac16e8271d --- /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-12-17 22:35:29Functions: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..1d9971049a --- /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-12-17 22:35:29Functions: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&)153275
+
+
+ + + +
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..5ec80f8128 --- /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-12-17 22:35:29Functions: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&)153275
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..8cfd05dacd --- /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-12-17 22:35:29Functions: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      153281 :     double headingFromRot(const T& rot)
+      58             :     {
+      59      153281 :       const vec3_t rot_vec = rot*vec3_t::UnitX();
+      60      306562 :       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-12-17 22:35:29Functions: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&)2192
mrs_lib::UTM(double, double, double*, double*)6157
mrs_lib::UTMLetterDesignator(double)159296
mrs_lib::LLtoUTM(double, double, double&, double&, char*)159327
+
+
+ + + +
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..0bba56e13b --- /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-12-17 22:35:29Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::UTMLetterDesignator(double)159296
mrs_lib::UTM(double, double, double*, double*)6157
mrs_lib::LLtoUTM(double, double, double&, double&, char*)159327
mrs_lib::UTMtoLL(double, double, char const*, double&, double&)2192
+
+
+ + + +
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..c941921359 --- /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-12-17 22:35:29Functions: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        6157 :   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        6157 :     int cm = ((lon >= 0.0) ? ((int)lon - ((int)lon) % 6 + 3) : ((int)lon - ((int)lon) % 6 - 3));
+      63             : 
+      64             :     // convert degrees into radians
+      65        6157 :     double rlat  = lat * RADIANS_PER_DEGREE;
+      66        6157 :     double rlon  = lon * RADIANS_PER_DEGREE;
+      67        6157 :     double rlon0 = cm * RADIANS_PER_DEGREE;
+      68             : 
+      69             :     // compute trigonometric functions
+      70        6157 :     double slat = sin(rlat);
+      71        6157 :     double clat = cos(rlat);
+      72        6157 :     double tlat = tan(rlat);
+      73             : 
+      74             :     // decide the false northing at origin
+      75        6157 :     double fn = (lat > 0) ? UTM_FN_N : UTM_FN_S;
+      76             : 
+      77        6157 :     double T = tlat * tlat;
+      78        6157 :     double C = UTM_EP2 * clat * clat;
+      79        6157 :     double A = (rlon - rlon0) * clat;
+      80        6157 :     double M = WGS84_A * (m0 * rlat + m1 * sin(2 * rlat) + m2 * sin(4 * rlat) + m3 * sin(6 * rlat));
+      81        6157 :     double V = WGS84_A / sqrt(1 - UTM_E2 * slat * slat);
+      82             : 
+      83             :     // compute the easting-northing coordinates
+      84        6157 :     *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        6121 :     *y = fn +
+      86        6121 :          UTM_K0 *
+      87        6135 :              (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        6121 :     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      159296 :   static inline char UTMLetterDesignator(double Lat) {
+     102             :     char LetterDesignator;
+     103             : 
+     104      159296 :     if ((84 >= Lat) && (Lat >= 72))
+     105           0 :       LetterDesignator = 'X';
+     106      159296 :     else if ((72 > Lat) && (Lat >= 64))
+     107           0 :       LetterDesignator = 'W';
+     108      159296 :     else if ((64 > Lat) && (Lat >= 56))
+     109           0 :       LetterDesignator = 'V';
+     110      159296 :     else if ((56 > Lat) && (Lat >= 48))
+     111           0 :       LetterDesignator = 'U';
+     112      159296 :     else if ((48 > Lat) && (Lat >= 40))
+     113      158415 :       LetterDesignator = 'T';
+     114         881 :     else if ((40 > Lat) && (Lat >= 32))
+     115           0 :       LetterDesignator = 'S';
+     116         881 :     else if ((32 > Lat) && (Lat >= 24))
+     117           0 :       LetterDesignator = 'R';
+     118         881 :     else if ((24 > Lat) && (Lat >= 16))
+     119           0 :       LetterDesignator = 'Q';
+     120         881 :     else if ((16 > Lat) && (Lat >= 8))
+     121           0 :       LetterDesignator = 'P';
+     122         881 :     else if ((8 > Lat) && (Lat >= 0))
+     123           2 :       LetterDesignator = 'N';
+     124         879 :     else if ((0 > Lat) && (Lat >= -8))
+     125           0 :       LetterDesignator = 'M';
+     126         879 :     else if ((-8 > Lat) && (Lat >= -16))
+     127           0 :       LetterDesignator = 'L';
+     128         879 :     else if ((-16 > Lat) && (Lat >= -24))
+     129           0 :       LetterDesignator = 'K';
+     130         879 :     else if ((-24 > Lat) && (Lat >= -32))
+     131           0 :       LetterDesignator = 'J';
+     132         879 :     else if ((-32 > Lat) && (Lat >= -40))
+     133           0 :       LetterDesignator = 'H';
+     134         879 :     else if ((-40 > Lat) && (Lat >= -48))
+     135           0 :       LetterDesignator = 'G';
+     136         879 :     else if ((-48 > Lat) && (Lat >= -56))
+     137           0 :       LetterDesignator = 'F';
+     138         879 :     else if ((-56 > Lat) && (Lat >= -64))
+     139           0 :       LetterDesignator = 'E';
+     140         879 :     else if ((-64 > Lat) && (Lat >= -72))
+     141           0 :       LetterDesignator = 'D';
+     142         879 :     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         879 :       LetterDesignator = 'Z';
+     147      159296 :     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      159327 :   static inline void LLtoUTM(const double Lat, const double Long, double& UTMNorthing, double& UTMEasting, char* UTMZone) {
+     160      159327 :     double a          = WGS84_A;
+     161      159327 :     double eccSquared = UTM_E2;
+     162      159327 :     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      159327 :     double LongTemp = (Long + 180) - int((Long + 180) / 360) * 360 - 180;
+     170             : 
+     171      159327 :     double LatRad  = Lat * RADIANS_PER_DEGREE;
+     172      159327 :     double LongRad = LongTemp * RADIANS_PER_DEGREE;
+     173             :     double LongOriginRad;
+     174             :     int    ZoneNumber;
+     175             : 
+     176      159327 :     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      159327 :     if (ZoneNumber > 99)
+     180           0 :       ZoneNumber = 99;
+     181      159327 :     if (ZoneNumber < -9)
+     182           0 :       ZoneNumber = -9;
+     183             : 
+     184      159327 :     if (Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0)
+     185           0 :       ZoneNumber = 32;
+     186             : 
+     187             :     // Special zones for Svalbard
+     188      159327 :     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      159327 :     LongOrigin    = (ZoneNumber - 1) * 6 - 180 + 3;
+     200      159327 :     LongOriginRad = LongOrigin * RADIANS_PER_DEGREE;
+     201             : 
+     202             :     // compute the UTM Zone from the latitude and longitude
+     203      159327 :     snprintf(UTMZone, 4, "%d%c", ZoneNumber, UTMLetterDesignator(Lat));
+     204             : 
+     205      158715 :     eccPrimeSquared = (eccSquared) / (1 - eccSquared);
+     206             : 
+     207      158715 :     N = a / sqrt(1 - eccSquared * sin(LatRad) * sin(LatRad));
+     208      158715 :     T = tan(LatRad) * tan(LatRad);
+     209      158715 :     C = eccPrimeSquared * cos(LatRad) * cos(LatRad);
+     210      158715 :     A = cos(LatRad) * (LongRad - LongOriginRad);
+     211             : 
+     212      158715 :     M = a * ((1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - 5 * eccSquared * eccSquared * eccSquared / 256) * LatRad -
+     213      158715 :              (3 * eccSquared / 8 + 3 * eccSquared * eccSquared / 32 + 45 * eccSquared * eccSquared * eccSquared / 1024) * sin(2 * LatRad) +
+     214      158715 :              (15 * eccSquared * eccSquared / 256 + 45 * eccSquared * eccSquared * eccSquared / 1024) * sin(4 * LatRad) -
+     215      158715 :              (35 * eccSquared * eccSquared * eccSquared / 3072) * sin(6 * LatRad));
+     216             : 
+     217      158715 :     UTMEasting =
+     218      158715 :         (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      158715 :     UTMNorthing = (double)(k0 * (M + N * tan(LatRad) *
+     221      158715 :                                          (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 +
+     222      158715 :                                           (61 - 58 * T + T * T + 600 * C - 330 * eccPrimeSquared) * A * A * A * A * A * A / 720)));
+     223      158715 :     if (Lat < 0)
+     224           0 :       UTMNorthing += 10000000.0;  // 10000000 meter offset for southern hemisphere
+     225      158715 :   }
+     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        2192 :   static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting, const char* UTMZone, double& Lat, double& Long) {
+     246        2192 :     double                  k0         = UTM_K0;
+     247        2192 :     double                  a          = WGS84_A;
+     248        2192 :     double                  eccSquared = UTM_E2;
+     249             :     double                  eccPrimeSquared;
+     250        2192 :     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        2192 :     x = UTMEasting - 500000.0;  // remove 500,000 meter offset for longitude
+     261        2192 :     y = UTMNorthing;
+     262             : 
+     263        2192 :     ZoneNumber = strtoul(UTMZone, &ZoneLetter, 10);
+     264        2192 :     if ((*ZoneLetter - 'N') >= 0)
+     265        2192 :       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        2192 :     LongOrigin = (ZoneNumber - 1) * 6 - 180 + 3;  //+3 puts origin in middle of zone
+     272             : 
+     273        2192 :     eccPrimeSquared = (eccSquared) / (1 - eccSquared);
+     274             : 
+     275        2192 :     M  = y / k0;
+     276        2192 :     mu = M / (a * (1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - 5 * eccSquared * eccSquared * eccSquared / 256));
+     277             : 
+     278        2192 :     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        2192 :               (151 * e1 * e1 * e1 / 96) * sin(6 * mu);
+     280        2192 :     phi1 = phi1Rad * DEGREES_PER_RADIAN;
+     281             : 
+     282        2192 :     N1 = a / sqrt(1 - eccSquared * sin(phi1Rad) * sin(phi1Rad));
+     283        2192 :     T1 = tan(phi1Rad) * tan(phi1Rad);
+     284        2192 :     C1 = eccPrimeSquared * cos(phi1Rad) * cos(phi1Rad);
+     285        2192 :     R1 = a * (1 - eccSquared) / pow(1 - eccSquared * sin(phi1Rad) * sin(phi1Rad), 1.5);
+     286        2192 :     D  = x / (N1 * k0);
+     287             : 
+     288        2192 :     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        2192 :                                                 (61 + 90 * T1 + 298 * C1 + 45 * T1 * T1 - 252 * eccPrimeSquared - 3 * C1 * C1) * D * D * D * D * D * D / 720);
+     290        2192 :     Lat = Lat * DEGREES_PER_RADIAN;
+     291             : 
+     292        2192 :     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        2192 :            cos(phi1Rad);
+     294        2192 :     Long = LongOrigin + Long * DEGREES_PER_RADIAN;
+     295        2192 :   }
+     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:36646678.5 %
Date:2024-12-17 22:35:29Functions:835150655.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 +
74.0%74.0%
+ 		74.0 %97 / 13143.6 %500 / 1147
<unnamed>74.0 %97 / 13143.6 %500 / 1147
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
service_client_handler.hpp +
80.0%80.0%
+ 		80.0 %52 / 6592.2 %47 / 51
<unnamed>80.0 %52 / 6592.2 %47 / 51
publisher_handler.hpp +
81.5%81.5%
+ 		81.5 %44 / 5495.3 %201 / 211
<unnamed>81.5 %44 / 5495.3 %201 / 211
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
+
+
+ + + + +
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..aaee99a4bc --- /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:36646678.5 %
Date:2024-12-17 22:35:29Functions:835150655.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
subscribe_handler.hpp +
74.0%74.0%
+ 		74.0 %97 / 13143.6 %500 / 1147
<unnamed>74.0 %97 / 13143.6 %500 / 1147
service_client_handler.hpp +
80.0%80.0%
+ 		80.0 %52 / 6592.2 %47 / 51
<unnamed>80.0 %52 / 6592.2 %47 / 51
publisher_handler.hpp +
81.5%81.5%
+ 		81.5 %44 / 5495.3 %201 / 211
<unnamed>81.5 %44 / 5495.3 %201 / 211
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
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..8393fb6c99 --- /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:36646678.5 %
Date:2024-12-17 22:35:29Functions:835150655.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 / 5495.3 %201 / 211
<unnamed>81.5 %44 / 5495.3 %201 / 211
service_client_handler.hpp +
80.0%80.0%
+ 		80.0 %52 / 6592.2 %47 / 51
<unnamed>80.0 %52 / 6592.2 %47 / 51
subscribe_handler.hpp +
74.0%74.0%
+ 		74.0 %97 / 13143.6 %500 / 1147
<unnamed>74.0 %97 / 13143.6 %500 / 1147
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..3c16c4de26 --- /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:36646678.5 %
Date:2024-12-17 22:35:29Functions:835150655.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 +
74.0%74.0%
+ 		74.0 %97 / 13143.6 %500 / 1147
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
service_client_handler.hpp +
80.0%80.0%
+ 		80.0 %52 / 6592.2 %47 / 51
publisher_handler.hpp +
81.5%81.5%
+ 		81.5 %44 / 5495.3 %201 / 211
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
+
+
+ + + + +
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..52ea8b97b6 --- /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:36646678.5 %
Date:2024-12-17 22:35:29Functions:835150655.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
subscribe_handler.hpp +
74.0%74.0%
+ 		74.0 %97 / 13143.6 %500 / 1147
service_client_handler.hpp +
80.0%80.0%
+ 		80.0 %52 / 6592.2 %47 / 51
publisher_handler.hpp +
81.5%81.5%
+ 		81.5 %44 / 5495.3 %201 / 211
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
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..ac40c2ff68 --- /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:36646678.5 %
Date:2024-12-17 22:35:29Functions:835150655.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 / 5495.3 %201 / 211
service_client_handler.hpp +
80.0%80.0%
+ 		80.0 %52 / 6592.2 %47 / 51
subscribe_handler.hpp +
74.0%74.0%
+ 		74.0 %97 / 13143.6 %500 / 1147
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..723082eb4b --- /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-12-17 22:35:29Functions: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> >&) const1951
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> >&) const1951
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> > > >&) const3808
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> > > >&) const3808
bool mrs_lib::ParamProvider::getParamImpl<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const5505
bool mrs_lib::ParamProvider::getParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const5505
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> >&) const18634
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> >&) const18634
bool mrs_lib::ParamProvider::getParamImpl<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&) const22398
bool mrs_lib::ParamProvider::getParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&) const22398
bool mrs_lib::ParamProvider::getParamImpl<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const44602
bool mrs_lib::ParamProvider::getParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const44602
+
+
+ + + +
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..71922514d8 --- /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-12-17 22:35:29Functions: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> >&) const18634
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> > > >&) const3808
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> >&) const1951
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&) const22398
bool mrs_lib::ParamProvider::getParamImpl<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const44602
bool mrs_lib::ParamProvider::getParamImpl<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const5505
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> >&) const18634
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> > > >&) const3808
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> >&) const1951
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&) const22398
bool mrs_lib::ParamProvider::getParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const44602
bool mrs_lib::ParamProvider::getParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const5505
+
+
+ + + +
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..2d543c42d0 --- /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-12-17 22:35:29Functions: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       96913 :   bool ParamProvider::getParam(const std::string& param_name, T& value_out) const
+      10             :   {
+      11             :     try
+      12             :     {
+      13       96913 :       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       96913 :   bool ParamProvider::getParamImpl(const std::string& param_name, T& value_out) const
+      24             :   {
+      25             :     {
+      26       96913 :       const auto found_node = findYamlNode(param_name);
+      27       96913 :       if (found_node.has_value())
+      28             :       {
+      29             :         try
+      30             :         {
+      31             :           // try catch is the only type-generic option...
+      32       82818 :           value_out = found_node.value().as<T>();
+      33       82818 :           return true;
+      34             :         }
+      35           0 :         catch (const YAML::BadConversion& e)
+      36             :         {}
+      37             :       }
+      38             : 
+      39             :     }
+      40             : 
+      41       14095 :     if (m_use_rosparam)
+      42       14095 :       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-12-17 22:35:29Functions:20121195.3 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::PublisherHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::publish(geometry_msgs::PointStamped_<std::allocator<void> > const&)0
mrs_lib::PublisherHandler<geometry_msgs::PointStamped_<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<geometry_msgs::PointStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PointStamped_<std::allocator<void> > > const&)0
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<geometry_msgs::PointStamped_<std::allocator<void> > >::publish(geometry_msgs::PointStamped_<std::allocator<void> > const&)0
mrs_lib::PublisherHandler_impl<geometry_msgs::PointStamped_<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::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&)32
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > > const&)32
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&)32
mrs_lib::PublisherHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::publish(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)92
mrs_lib::PublisherHandler_impl<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::publish(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)92
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&)109
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > > const&)109
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&)109
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > > const&)109
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&)109
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)109
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&)109
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > > const&)109
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&)109
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&)109
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&)109
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&)109
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > > const&)109
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&)109
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&)109
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&)109
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > > const&)109
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&)109
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > > const&)109
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&)109
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > > const&)109
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&)109
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > > const&)109
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&)109
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > > const&)109
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&)109
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&)109
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&)109
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > > const&)109
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&)109
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&)109
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > > const&)109
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&)109
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > > const&)109
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&)109
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > > const&)109
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&)109
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > > const&)109
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&)109
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&)109
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&)109
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&)109
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&)109
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&)109
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&)109
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&)109
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&)109
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&)109
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&)109
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&)109
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&)109
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&)109
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&)109
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&)109
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&)109
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&)109
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&)109
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&)109
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&)109
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&)109
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&)109
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&)110
mrs_lib::PublisherHandler<std_msgs::Int64_<std::allocator<void> > >::publish(std_msgs::Int64_<std::allocator<void> > const&)110
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&)110
mrs_lib::PublisherHandler_impl<std_msgs::Int64_<std::allocator<void> > >::publish(std_msgs::Int64_<std::allocator<void> > const&)110
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&)116
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > > const&)116
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&)116
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&)218
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > > const&)218
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > > const&)218
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > > const&)218
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > > const&)218
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > > const&)218
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > > const&)218
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > > const&)218
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > > const&)218
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > > const&)218
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&)218
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > > const&)219
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&)226
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > > const&)226
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&)226
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&)312
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > > const&)312
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&)312
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&)428
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > > const&)428
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&)428
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::publish(mrs_msgs::ObstacleSectors_<std::allocator<void> > const&)435
mrs_lib::PublisherHandler_impl<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::publish(mrs_msgs::ObstacleSectors_<std::allocator<void> > const&)435
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&)436
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > > const&)436
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&)436
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > > const&)436
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&)436
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&)436
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > > const&)445
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)508
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)508
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&)528
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&)528
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&)625
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > > const&)625
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&)625
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&)654
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > > const&)654
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&)654
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&)804
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > > const&)804
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&)804
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1086
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1086
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1137
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1137
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const&)1862
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const&)1862
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const&)1865
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const&)1865
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)2259
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const&)2259
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)2259
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const&)2259
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::publish(mrs_msgs::FutureTrajectory_<std::allocator<void> > const&)2342
mrs_lib::PublisherHandler_impl<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::publish(mrs_msgs::FutureTrajectory_<std::allocator<void> > const&)2342
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2474
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2474
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3976
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3976
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)4022
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)4022
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::publish(mrs_msgs::HwApiRcChannels_<std::allocator<void> > const&)7359
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::publish(mrs_msgs::HwApiRcChannels_<std::allocator<void> > const&)7359
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)8249
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)8249
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::publish(std_msgs::Bool_<std::allocator<void> > const&)10072
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::publish(std_msgs::Bool_<std::allocator<void> > const&)10072
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::publish(mrs_msgs::ControlError_<std::allocator<void> > const&)11991
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::publish(mrs_msgs::ControlError_<std::allocator<void> > const&)11991
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::publish(std_msgs::Empty_<std::allocator<void> > const&)13140
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::publish(std_msgs::Empty_<std::allocator<void> > const&)13140
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::publish(mrs_msgs::DynamicsConstraints_<std::allocator<void> > const&)16246
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::publish(mrs_msgs::DynamicsConstraints_<std::allocator<void> > const&)16246
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const&)19088
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const&)19088
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const&)19545
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const&)19545
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const&)24619
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::publish(std_msgs::String_<std::allocator<void> > const&)24619
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const&)24619
mrs_lib::PublisherHandler_impl<std_msgs::String_<std::allocator<void> > >::publish(std_msgs::String_<std::allocator<void> > const&)24619
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::publish(visualization_msgs::MarkerArray_<std::allocator<void> > const&)37145
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::publish(visualization_msgs::MarkerArray_<std::allocator<void> > const&)37145
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorInput_<std::allocator<void> > const&)96740
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorInput_<std::allocator<void> > const&)96740
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::publish(geometry_msgs::PoseStamped_<std::allocator<void> > const&)100263
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::publish(geometry_msgs::PoseStamped_<std::allocator<void> > const&)100263
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)103535
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)103535
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::publish(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)111342
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::publish(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)111342
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler(mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > > const&)132489
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const&)151825
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const&)151825
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::publish(geometry_msgs::PoseArray_<std::allocator<void> > const&)169790
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::publish(geometry_msgs::PoseArray_<std::allocator<void> > const&)169790
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::publish(geometry_msgs::QuaternionStamped_<std::allocator<void> > const&)218316
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::publish(geometry_msgs::QuaternionStamped_<std::allocator<void> > const&)218320
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::publish(std_msgs::Float64_<std::allocator<void> > const&)252220
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::publish(std_msgs::Float64_<std::allocator<void> > const&)252220
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::publish(mrs_msgs::Float64Stamped_<std::allocator<void> > const&)332483
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::publish(mrs_msgs::Float64Stamped_<std::allocator<void> > const&)332507
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::publish(mrs_msgs::UavState_<std::allocator<void> > const&)387817
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::publish(mrs_msgs::UavState_<std::allocator<void> > const&)387828
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::publish(mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)510756
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::publish(mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)511033
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorOutput_<std::allocator<void> > const&)729312
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorOutput_<std::allocator<void> > const&)729314
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::publish(nav_msgs::Odometry_<std::allocator<void> > const&)798725
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::publish(nav_msgs::Odometry_<std::allocator<void> > const&)798737
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::publish(mrs_msgs::EstimatorCorrection_<std::allocator<void> > const&)1459204
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::publish(mrs_msgs::EstimatorCorrection_<std::allocator<void> > const&)1459235
+
+
+ + + +
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..000b8f2dc7 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.func.html @@ -0,0 +1,924 @@ + + + + + + + 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-12-17 22:35:29Functions:20121195.3 %
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&)169790
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&)436
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > > const&)436
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::publish(geometry_msgs::PoseStamped_<std::allocator<void> > const&)100263
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&)218
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > > const&)218
mrs_lib::PublisherHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::publish(geometry_msgs::PointStamped_<std::allocator<void> > const&)0
mrs_lib::PublisherHandler<geometry_msgs::PointStamped_<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<geometry_msgs::PointStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PointStamped_<std::allocator<void> > > const&)0
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::publish(geometry_msgs::QuaternionStamped_<std::allocator<void> > const&)218320
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&)116
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > > const&)116
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::publish(visualization_msgs::MarkerArray_<std::allocator<void> > const&)37145
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&)654
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > > const&)654
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::publish(mrs_msgs::ControlError_<std::allocator<void> > const&)11991
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&)109
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > > const&)109
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorInput_<std::allocator<void> > const&)96740
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&)109
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > > const&)109
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::publish(mrs_msgs::Float64Stamped_<std::allocator<void> > const&)332507
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&)625
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > > const&)625
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::publish(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)111342
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&)109
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)109
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorOutput_<std::allocator<void> > const&)729312
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&)428
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > > const&)428
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::publish(mrs_msgs::HwApiRcChannels_<std::allocator<void> > const&)7359
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&)435
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&)2342
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&)109
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > > const&)109
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)4022
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&)109
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > > const&)218
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)8249
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&)109
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > > const&)218
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)508
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&)109
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > > const&)218
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::publish(mrs_msgs::DynamicsConstraints_<std::allocator<void> > const&)16246
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&)109
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > > const&)109
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::publish(mrs_msgs::EstimatorCorrection_<std::allocator<void> > const&)1459235
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&)804
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > > const&)804
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::publish(mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)511033
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&)312
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > > const&)312
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)2259
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&)110
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > > const&)219
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&)103535
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&)109
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > > const&)218
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3976
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&)109
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > > const&)218
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const&)151825
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&)109
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > > const&)109
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const&)19088
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&)109
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > > const&)109
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const&)24619
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&)109
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > > const&)109
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const&)2259
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&)109
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > > const&)109
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const&)1862
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&)109
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > > const&)109
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1086
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&)109
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > > const&)218
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2474
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&)109
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > > const&)218
mrs_lib::PublisherHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::publish(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)92
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const&)19545
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&)109
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > > const&)109
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1137
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&)109
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > > const&)218
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const&)1865
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&)109
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > > const&)109
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&)109
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > > const&)109
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::publish(mrs_msgs::UavState_<std::allocator<void> > const&)387828
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&)226
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > > const&)226
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::publish(nav_msgs::Odometry_<std::allocator<void> > const&)798737
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler(mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > > const&)132489
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&)528
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > > const&)445
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::publish(std_msgs::Bool_<std::allocator<void> > const&)10072
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&)32
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > > const&)32
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::publish(std_msgs::Empty_<std::allocator<void> > const&)13140
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&)109
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > > const&)109
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&)24619
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&)109
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > > const&)109
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::publish(std_msgs::Float64_<std::allocator<void> > const&)252220
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&)436
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > > const&)436
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::publish(geometry_msgs::PoseArray_<std::allocator<void> > const&)169790
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&)436
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::publish(geometry_msgs::PoseStamped_<std::allocator<void> > const&)100263
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&)218
mrs_lib::PublisherHandler_impl<geometry_msgs::PointStamped_<std::allocator<void> > >::publish(geometry_msgs::PointStamped_<std::allocator<void> > const&)0
mrs_lib::PublisherHandler_impl<geometry_msgs::PointStamped_<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<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::publish(geometry_msgs::QuaternionStamped_<std::allocator<void> > const&)218316
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&)116
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::publish(visualization_msgs::MarkerArray_<std::allocator<void> > const&)37145
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&)654
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::publish(mrs_msgs::ControlError_<std::allocator<void> > const&)11991
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&)109
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorInput_<std::allocator<void> > const&)96740
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&)109
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::publish(mrs_msgs::Float64Stamped_<std::allocator<void> > const&)332483
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&)625
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::publish(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)111342
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&)109
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorOutput_<std::allocator<void> > const&)729314
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&)428
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::publish(mrs_msgs::HwApiRcChannels_<std::allocator<void> > const&)7359
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&)435
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&)2342
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&)109
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)4022
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&)109
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)8249
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&)109
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)508
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&)109
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::publish(mrs_msgs::DynamicsConstraints_<std::allocator<void> > const&)16246
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&)109
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::publish(mrs_msgs::EstimatorCorrection_<std::allocator<void> > const&)1459204
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&)804
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::publish(mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)510756
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&)312
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)2259
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&)110
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&)103535
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&)109
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3976
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&)109
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const&)151825
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&)109
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const&)19088
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&)109
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const&)24619
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&)109
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const&)2259
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&)109
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const&)1862
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&)109
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1086
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&)109
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2474
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&)109
mrs_lib::PublisherHandler_impl<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::publish(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)92
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const&)19545
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&)109
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1137
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&)109
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const&)1865
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&)109
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&)109
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::publish(mrs_msgs::UavState_<std::allocator<void> > const&)387817
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&)226
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::publish(nav_msgs::Odometry_<std::allocator<void> > const&)798725
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&)528
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::publish(std_msgs::Bool_<std::allocator<void> > const&)10072
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&)32
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::publish(std_msgs::Empty_<std::allocator<void> > const&)13140
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&)109
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&)24619
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&)109
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::publish(std_msgs::Float64_<std::allocator<void> > const&)252220
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&)436
+
+
+ + + +
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..a30d8b892e --- /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-12-17 22:35:29Functions:20121195.3 %
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        7437 : PublisherHandler_impl<TopicType>::PublisherHandler_impl(ros::NodeHandle& nh, const std::string& address, const unsigned int& buffer_size, const bool& latch,
+      23        7437 :                                                         const double& rate) {
+      24             : 
+      25             :   {
+      26        7437 :     std::scoped_lock lock(mutex_publisher_);
+      27             : 
+      28        7437 :     publisher_ = nh.advertise<TopicType>(address, buffer_size, latch);
+      29             : 
+      30        7437 :     if (rate > 0.0) {
+      31             : 
+      32         764 :       throttle_ = true;
+      33             : 
+      34         764 :       throttle_min_dt_ = 1.0 / rate;
+      35             : 
+      36             :     } else {
+      37             : 
+      38        6673 :       throttle_ = false;
+      39             : 
+      40        6673 :       throttle_min_dt_ = 0;
+      41             :     }
+      42             : 
+      43        7437 :     last_time_published_ = ros::Time(0);
+      44             :   }
+      45             : 
+      46        7437 :   publisher_initialized_ = true;
+      47        7437 : }
+      48             : 
+      49             : //}
+      50             : 
+      51             : /* publish(TopicType& msg) //{ */
+      52             : 
+      53             : template <class TopicType>
+      54     5638837 : void PublisherHandler_impl<TopicType>::publish(const TopicType& msg) {
+      55             : 
+      56     5638837 :   if (!publisher_initialized_) {
+      57           0 :     return;
+      58             :   }
+      59             : 
+      60             :   {
+      61     5636388 :     std::scoped_lock lock(mutex_publisher_);
+      62             : 
+      63     5636074 :     if (throttle_) {
+      64             : 
+      65      304838 :       if ((ros::Time::now() - last_time_published_).toSec() < throttle_min_dt_) {
+      66      198391 :         return;
+      67             :       }
+      68             : 
+      69      106447 :       last_time_published_ = ros::Time::now();
+      70             :     }
+      71             : 
+      72             :     try {
+      73     5437683 :       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        8333 : PublisherHandler<TopicType>& PublisherHandler<TopicType>::operator=(const PublisherHandler<TopicType>& other) {
+     169             : 
+     170        8333 :   if (this == &other) {
+     171           0 :     return *this;
+     172             :   }
+     173             : 
+     174        8333 :   if (other.impl_) {
+     175        8333 :     this->impl_ = other.impl_;
+     176             :   }
+     177             : 
+     178        8333 :   return *this;
+     179             : }
+     180             : 
+     181             : //}
+     182             : 
+     183             : /* copy constructor //{ */
+     184             : 
+     185             : template <class TopicType>
+     186      132489 : PublisherHandler<TopicType>::PublisherHandler(const PublisherHandler<TopicType>& other) {
+     187             : 
+     188      132489 :   if (other.impl_) {
+     189      132489 :     this->impl_ = other.impl_;
+     190             :   }
+     191      132489 : }
+     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        7437 : PublisherHandler<TopicType>::PublisherHandler(ros::NodeHandle& nh, const std::string& address, const unsigned int& buffer_size, const bool& latch,
+     199        7437 :                                               const double& rate) {
+     200             : 
+     201        7437 :   impl_ = std::make_shared<PublisherHandler_impl<TopicType>>(nh, address, buffer_size, latch, rate);
+     202        7437 : }
+     203             : 
+     204             : //}
+     205             : 
+     206             : /* publish(const TopicType& msg) //{ */
+     207             : 
+     208             : template <class TopicType>
+     209     5639194 : void PublisherHandler<TopicType>::publish(const TopicType& msg) {
+     210             : 
+     211     5639194 :   impl_->publish(msg);
+     212     5640184 : }
+     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..9627cdb9a7 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func-sort-c.html @@ -0,0 +1,284 @@ + + + + + + + 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:526580.0 %
Date:2024-12-17 22:35:29Functions:475192.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ServiceClientHandler<mrs_msgs::String>::callAsync(mrs_msgs::String&)0
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::call(mrs_msgs::String&, int const&, double const&)0
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::asyncRun()0
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::callAsync(mrs_msgs::String&)0
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&)21
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::call(mrs_msgs::Vec1&)21
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::call(mrs_msgs::ReferenceStampedSrv&)102
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::call(mrs_msgs::ReferenceStampedSrv&)102
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)109
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv> const&)109
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)109
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::Vec1> const&)109
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::initialize(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)109
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)109
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)109
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)111
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv> const&)111
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)111
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::call(mrs_msgs::DynamicsConstraintsSrv&)112
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::call(mrs_msgs::DynamicsConstraintsSrv&)112
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::call(std_srvs::Trigger&)177
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::call(std_srvs::Trigger&)177
mrs_lib::ServiceClientHandler<mrs_msgs::String>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)327
mrs_lib::ServiceClientHandler<mrs_msgs::String>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::String> const&)327
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)327
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::call(std_srvs::SetBool&)364
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::call(std_srvs::SetBool&)364
mrs_lib::ServiceClientHandler<mrs_msgs::String>::call(mrs_msgs::String&)427
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::call(mrs_msgs::String&)427
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)656
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::operator=(mrs_lib::ServiceClientHandler<std_srvs::SetBool> const&)656
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)656
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1083
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::operator=(mrs_lib::ServiceClientHandler<std_srvs::Trigger> const&)1083
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1192
+
+
+ + + +
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..2f856dcffc --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func.html @@ -0,0 +1,284 @@ + + + + + + + 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:526580.0 %
Date:2024-12-17 22:35:29Functions:475192.2 %
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&)102
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)111
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv> const&)111
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::call(mrs_msgs::DynamicsConstraintsSrv&)112
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)109
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv> const&)109
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::call(mrs_msgs::Vec1&)21
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)109
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::Vec1> const&)109
mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>::call(mrs_msgs::Vec4&)1
mrs_lib::ServiceClientHandler<mrs_msgs::String>::call(mrs_msgs::String&)427
mrs_lib::ServiceClientHandler<mrs_msgs::String>::callAsync(mrs_msgs::String&)0
mrs_lib::ServiceClientHandler<mrs_msgs::String>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)327
mrs_lib::ServiceClientHandler<mrs_msgs::String>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::String> const&)327
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::call(std_srvs::SetBool&)364
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)656
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::operator=(mrs_lib::ServiceClientHandler<std_srvs::SetBool> const&)656
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::initialize(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)109
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::call(std_srvs::Trigger&)177
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&)1083
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::operator=(mrs_lib::ServiceClientHandler<std_srvs::Trigger> const&)1083
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&)102
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)111
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::call(mrs_msgs::DynamicsConstraintsSrv&)112
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)109
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::call(mrs_msgs::Vec1&)21
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)109
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec4>::call(mrs_msgs::Vec4&)1
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::call(mrs_msgs::String&)427
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::call(mrs_msgs::String&, int const&, double const&)0
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::asyncRun()0
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::callAsync(mrs_msgs::String&)0
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)327
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::call(std_srvs::SetBool&)364
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)656
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::call(std_srvs::Trigger&)177
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&)1192
+
+
+ + + +
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..a074d149e5 --- /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:526580.0 %
Date:2024-12-17 22:35:29Functions:475192.2 %
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        2507 : ServiceClientHandler_impl<ServiceType>::ServiceClientHandler_impl(ros::NodeHandle& nh, const std::string& address) {
+      23             : 
+      24             :   {
+      25        2507 :     std::scoped_lock lock(mutex_service_client_);
+      26             : 
+      27        2507 :     service_client_ = nh.serviceClient<ServiceType>(address);
+      28             :   }
+      29             : 
+      30        2507 :   _address_       = address;
+      31        2507 :   async_attempts_ = 1;
+      32             : 
+      33             :   /* thread_oneshot_ = std::make_shared<std::thread>(std::thread(&ServiceClientHandler_impl::threadOneshot, this, true, false)); */
+      34             : 
+      35        2507 :   service_initialized_ = true;
+      36        2507 : }
+      37             : 
+      38             : //}
+      39             : 
+      40             : /* call(ServiceType& srv) //{ */
+      41             : 
+      42             : template <class ServiceType>
+      43        1216 : bool ServiceClientHandler_impl<ServiceType>::call(ServiceType& srv) {
+      44             : 
+      45        1216 :   if (!service_initialized_) {
+      46           0 :     return false;
+      47             :   }
+      48             : 
+      49        1216 :   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           0 : std::future<ServiceType> ServiceClientHandler_impl<ServiceType>::callAsync(ServiceType& srv) {
+     124             : 
+     125             :   {
+     126           0 :     std::scoped_lock lock(mutex_async_);
+     127             : 
+     128           0 :     async_data_         = srv;
+     129           0 :     async_attempts_     = 1;
+     130           0 :     async_repeat_delay_ = 0;
+     131             :   }
+     132             : 
+     133           0 :   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        2398 : ServiceClientHandler<ServiceType>& ServiceClientHandler<ServiceType>::operator=(const ServiceClientHandler<ServiceType>& other) {
+     205             : 
+     206        2398 :   if (this == &other) {
+     207           0 :     return *this;
+     208             :   }
+     209             : 
+     210        2398 :   if (other.impl_) {
+     211        2398 :     this->impl_ = other.impl_;
+     212             :   }
+     213             : 
+     214        2398 :   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        2398 : ServiceClientHandler<ServiceType>::ServiceClientHandler(ros::NodeHandle& nh, const std::string& address) {
+     235             : 
+     236        2398 :   impl_ = std::make_shared<ServiceClientHandler_impl<ServiceType>>(nh, address);
+     237        2398 : }
+     238             : 
+     239             : //}
+     240             : 
+     241             : /* initialize(ros::NodeHandle& nh, const std::string& address) //{ */
+     242             : 
+     243             : template <class ServiceType>
+     244         109 : void ServiceClientHandler<ServiceType>::initialize(ros::NodeHandle& nh, const std::string& address) {
+     245             : 
+     246         109 :   impl_ = std::make_shared<ServiceClientHandler_impl<ServiceType>>(nh, address);
+     247         109 : }
+     248             : 
+     249             : //}
+     250             : 
+     251             : /* call(ServiceType& srv) //{ */
+     252             : 
+     253             : template <class ServiceType>
+     254        1216 : bool ServiceClientHandler<ServiceType>::call(ServiceType& srv) {
+     255             : 
+     256        1216 :   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           0 : std::future<ServiceType> ServiceClientHandler<ServiceType>::callAsync(ServiceType& srv) {
+     285             : 
+     286           0 :   std::future<ServiceType> res = impl_->callAsync(srv);
+     287             : 
+     288           0 :   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..7db9e41fe5dacee7b0c82744a4bbdbdf01da5574 GIT binary patch literal 1023 zcmVPZx@b0n#64rA?&a^CjCfy1HnL|cVL+T43ycNg3Z;c-szatd!__iyx3KD4 z0wQ)DKS&xTB;ohMG&yh)iIQhTwCDp-%7h6rU_8sn8iS?DNzjrZ9leSrW3W0N;lsck zPBg~4MgZ0f$YmrL({gh$y`?H3;y0JLL}5T;4+7%yOcA2UGtTq2ZOgW;Gfcx=;&6;; zn$9~q6D9cQ!F#50^r$cPu+R}o9JrkieQsGZ79O-p77-hi*TcT37(PN|HaQkhMs67E z8D;&oQ~!r8@YSns>nseOfdzJPEqn<{qmuIka@tMDtN{u z>#2O?)ogV?6RwU(%cli}$ghfL?5!ZBD0SN$loJ2D=C{oi&O08Flv1J;lFlr|QIpUE z0(j{1)lrj(5@z0*DcaKjW2?5Xh}+99HN^HL=snvfcC-rRC9X0eoK^*}sx; znVXy26{qF8``Ii(V*f{*<3b&m%b1 + + + + + + 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:9713174.0 %
Date:2024-12-17 22:35:29Functions:500114743.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> > >::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::setNoMessageTimeout(ros::Duration 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::MagneticField_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::MagneticField_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::ImplThreadsafe::start()0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::MagneticField_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().20
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<sensor_msgs::MagneticField_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::MagneticField_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<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::MagneticField_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::start()0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::MagneticField_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::~Impl().20
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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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> > >::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::setNoMessageTimeout(ros::Duration 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::~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::setNoMessageTimeout(ros::Duration 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::setNoMessageTimeout(ros::Duration 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::getNumPublishers() 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::MagneticField_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::getNumPublishers() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::getNumPublishers() 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::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::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::newMsg() 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> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::getNumPublishers() 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::getNumPublishers() 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::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)1
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)1
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<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&)2
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<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::getMsg()4
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::getMsg()4
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const4
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::peekMsg() const4
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<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::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const7
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::lastMsgTime() const7
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::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::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const16
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::hasMsg() const16
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const18
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const20
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const20
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()32
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&)32
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()32
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().232
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::start()32
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&)32
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::~Impl().232
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()32
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&)32
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()32
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().232
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::start()32
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&)32
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().232
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()32
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&)32
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()32
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().232
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::start()32
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&)32
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::~Impl().232
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const64
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const64
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const64
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const> const&)92
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const> const&)92
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::getMsg()103
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::getMsg()104
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const104
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const104
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::hasMsg() const104
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::peekMsg() const104
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const105
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const105
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const105
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const105
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()109
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2109
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::~Impl().2109
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()109
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2109
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::~Impl().2109
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()109
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2109
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::~Impl().2109
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()109
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2109
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::~Impl().2109
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()109
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2109
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::~Impl().2109
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()109
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2109
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::~Impl().2109
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()109
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2109
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::~Impl().2109
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()109
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2109
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().2109
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()109
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2109
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::~Impl().2109
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()109
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2109
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().2109
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()109
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2109
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::~Impl().2109
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const110
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const110
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const111
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&)113
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()113
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2113
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&)113
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl().2113
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::start()116
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&)116
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()116
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2116
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::start()116
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&)116
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::~Impl().2116
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::start()117
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::start()117
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const122
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const146
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::start()196
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&)196
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()196
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2196
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::start()196
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&)196
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::~Impl().2196
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const215
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::lastMsgTime() const215
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::start()218
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&)218
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()218
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2218
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::start()218
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&)218
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::~Impl().2218
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::start()218
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&)218
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()218
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2218
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::start()218
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&)218
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::~Impl().2218
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::start()218
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&)218
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()218
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2218
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::start()218
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&)218
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::~Impl().2218
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const218
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const218
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const218
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const218
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const218
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const218
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const218
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const218
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const218
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const218
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const218
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::start()222
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&)222
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()222
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2222
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::start()222
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&)222
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::~Impl().2222
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const226
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const232
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::start()250
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&)250
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()250
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2250
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::start()250
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&)250
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::~Impl().2250
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const276
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::lastMsgTime() const276
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::getMsg()308
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::getMsg()308
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const308
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::peekMsg() const308
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::start()327
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&)327
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()327
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2327
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::start()327
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&)327
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::~Impl().2327
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::start()337
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&)337
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()337
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2337
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::start()337
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&)337
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::~Impl().2337
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)348
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)348
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::start()352
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&)352
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()352
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2352
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::start()352
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&)352
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::~Impl().2352
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::start()359
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&)359
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()359
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2359
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::start()359
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&)359
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::~Impl().2359
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::getMsg()380
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::getMsg()380
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const380
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::peekMsg() const380
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)389
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)389
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const392
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::start()421
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&)421
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()421
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2421
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::start()421
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&)421
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::~Impl().2421
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::start()436
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&)436
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()436
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2436
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::start()436
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&)436
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::~Impl().2436
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::start()436
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&)436
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()436
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2436
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::start()436
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&)436
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::~Impl().2436
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const436
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const436
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const436
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const444
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()468
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&)468
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()468
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2468
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::start()468
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&)468
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::~Impl().2468
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()468
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&)468
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()468
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2468
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::start()468
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&)468
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().2468
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const500
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const> const&)591
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const> const&)591
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::start()650
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&)650
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()650
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2650
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::start()650
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&)650
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::~Impl().2650
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const654
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const674
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const704
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::getMsg()785
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::getMsg()785
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::getMsg()785
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::getMsg()785
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const785
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::peekMsg() const785
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const785
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const785
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::hasMsg() const785
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::peekMsg() const785
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const829
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const842
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const872
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const872
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const936
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const1058
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const1448
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const> const&)1861
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const> const&)1861
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const> const&)1862
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const> const&)1862
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)2189
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)2189
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const2760
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::hasMsg() const2760
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)3475
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)3475
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()4056
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::getMsg()4056
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const4056
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::peekMsg() const4056
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&)5713
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&)5719
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)8317
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)8332
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::getMsg()10220
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::getMsg()10220
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const10220
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const10220
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::hasMsg() const10220
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::peekMsg() const10220
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::getMsg()12935
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const12936
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::getMsg()12937
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::peekMsg() const12937
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const> const&)16233
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const> const&)16233
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const16847
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::hasMsg() const16849
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const17792
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::hasMsg() const17792
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const19396
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const19396
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const> const&)19444
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const> const&)19445
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const19629
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::hasMsg() const19629
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::getMsg()20520
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const20520
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::getMsg()20521
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::peekMsg() const20521
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const> const&)24855
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const> const&)24855
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const39903
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::hasMsg() const39903
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::getMsg()40939
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::getMsg()40939
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const40939
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const40939
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::lastMsgTime() const40939
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::peekMsg() const40939
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()50689
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const50694
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::peekMsg() const50694
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::getMsg()50695
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const58949
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::hasMsg() const58949
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const> const&)59413
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const> const&)59413
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::hasMsg() const74154
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const74204
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()75250
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::getMsg()75287
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const75288
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const75288
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const> const&)79955
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const> const&)80659
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)83332
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)83448
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const88585
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const88625
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const119815
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::hasMsg() const119815
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const> const&)151740
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const> const&)151740
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()152267
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const152278
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()152281
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const152281
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const154660
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const154675
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::getMsg()185781
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::getMsg()185781
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const185781
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::peekMsg() const185781
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const185787
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::hasMsg() const185787
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const210372
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::getMsg()210379
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::peekMsg() const210380
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()210383
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const> const&)216442
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const> const&)216454
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const> const&)216614
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const> const&)216732
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&)218549
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&)218558
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)222305
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)222360
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const232848
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::lastMsgTime() const232848
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::getMsg()250058
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::getMsg()250064
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const250064
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::peekMsg() const250064
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const271724
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::hasMsg() const271726
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const> const&)301144
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const> const&)301175
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const> const&)314044
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const> const&)314169
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const> const&)320409
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const> const&)320787
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&)390884
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&)391345
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::hasMsg() const435979
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const436198
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const444023
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::getMsg()444121
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::getMsg()444233
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::peekMsg() const444233
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::lastMsgTime() const464346
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const464524
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const> const&)527815
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const> const&)527943
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::newMsg() const562270
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::newMsg() const563023
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)566189
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)566249
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const588089
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::hasMsg() const588095
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const> const&)694649
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const> const&)695170
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()797011
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const797316
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::getMsg()797327
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::peekMsg() const797369
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::hasMsg() const1026427
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const1026535
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const> const&)1032894
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const> const&)1033138
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)1280137
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)1280671
+
+
+ + + +
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..80b32b58a9 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func.html @@ -0,0 +1,4668 @@ + + + + + + + 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:9713174.0 %
Date:2024-12-17 22:35:29Functions:500114743.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> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const> const&)320787
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::start()436
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&)436
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()436
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2436
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&)320409
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()436
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&)436
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::~Impl().2436
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::MagneticField_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::ImplThreadsafe::start()0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::MagneticField_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().20
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<sensor_msgs::MagneticField_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::MagneticField_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<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::MagneticField_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::start()0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::MagneticField_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::~Impl().20
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const> const&)59413
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::start()32
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::getMsg()10220
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&)32
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()32
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().232
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&)59413
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()32
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::getMsg()10220
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&)32
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::~Impl().232
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()218
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&)218
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()218
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2218
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::setNoMessageTimeout(ros::Duration 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()218
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&)218
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::~Impl().2218
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const> const&)314169
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::start()196
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::getMsg()12935
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&)196
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()196
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2196
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&)314044
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()196
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::getMsg()12937
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&)196
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::~Impl().2196
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::setNoMessageTimeout(ros::Duration 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&)218549
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::stop()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::start()117
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()4056
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&)113
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()113
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2113
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&)218558
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::stop()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::start()117
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::getMsg()4056
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&)113
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl().2113
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const> const&)694649
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::start()352
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()210383
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&)352
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()352
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2352
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&)695170
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()352
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::getMsg()210379
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&)352
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::~Impl().2352
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)1280137
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::start()650
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()797011
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&)650
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()650
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2650
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&)1280671
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration const&)0
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&)2
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::start()650
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::getMsg()797327
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&)650
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::~Impl().2650
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const> const&)1033138
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::start()250
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::getMsg()308
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&)250
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()250
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2250
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&)1032894
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()250
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::getMsg()308
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&)250
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::~Impl().2250
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const> const&)216732
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::start()222
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::getMsg()185781
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&)222
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()222
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2222
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&)216614
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()222
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::getMsg()185781
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&)222
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::~Impl().2222
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&)391345
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::start()421
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::getMsg()444233
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&)421
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()421
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2421
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&)390884
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()421
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::getMsg()444121
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&)421
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::~Impl().2421
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const> const&)301175
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::start()436
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()50689
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&)436
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()436
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2436
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&)301144
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()436
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::getMsg()50695
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&)436
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::~Impl().2436
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)222360
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::start()218
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::getMsg()785
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&)218
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()218
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2218
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&)222305
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()218
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::getMsg()785
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&)218
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::~Impl().2218
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const> const&)216454
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::start()116
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&)116
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()116
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2116
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&)216442
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()116
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&)116
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::~Impl().2116
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const> const&)24855
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::start()109
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::getMsg()785
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()109
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2109
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&)24855
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()109
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::getMsg()785
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::~Impl().2109
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)389
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::start()109
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::getMsg()380
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()109
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2109
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&)389
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()109
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::getMsg()380
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::~Impl().2109
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)348
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&)348
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::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&)1
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()109
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2109
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&)1
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::~Impl().2109
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&)5713
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::start()359
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::getMsg()20520
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&)359
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()359
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2359
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&)5719
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()359
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::getMsg()20521
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&)359
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::~Impl().2359
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const> const&)16233
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()109
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2109
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&)16233
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::~Impl().2109
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()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()109
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2109
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::setNoMessageTimeout(ros::Duration 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()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::~Impl().2109
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()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()109
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2109
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::setNoMessageTimeout(ros::Duration 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::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::~Impl().2109
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const> const&)151740
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()109
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2109
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&)151740
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::~Impl().2109
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const> const&)80659
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()468
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()75250
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&)468
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()468
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2468
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&)79955
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()468
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::getMsg()75287
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&)468
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::~Impl().2468
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)2189
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&)2189
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::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&)591
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()32
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&)32
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()32
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().232
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&)591
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()32
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&)32
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().232
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const> const&)1861
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()109
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2109
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&)1861
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().2109
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)3475
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()32
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&)32
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()32
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().232
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&)3475
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()32
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&)32
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::~Impl().232
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const> const&)92
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()109
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2109
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&)92
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::~Impl().2109
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)83448
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()468
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()152267
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&)468
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()468
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2468
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&)83332
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()468
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()152281
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&)468
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().2468
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const> const&)1862
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()109
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2109
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&)1862
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().2109
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()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()109
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2109
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::setNoMessageTimeout(ros::Duration 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::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::~Impl().2109
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)8332
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()104
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&)8317
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::getMsg()103
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&)527815
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::start()327
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::getMsg()4
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&)327
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()327
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2327
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&)527943
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()327
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::getMsg()4
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&)327
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::~Impl().2327
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)566189
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::start()337
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::getMsg()250058
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&)337
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()337
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2337
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&)566249
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()337
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::getMsg()250064
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&)337
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::~Impl().2337
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const> const&)19445
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::start()218
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::getMsg()40939
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&)218
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()218
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2218
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&)19444
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::setNoMessageTimeout(ros::Duration 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::start()218
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::getMsg()40939
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&)218
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::~Impl().2218
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::getNumPublishers() 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]() const872
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::getNumPublishers() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const10220
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const10220
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::getNumPublishers() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::hasMsg() const10220
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::peekMsg() const10220
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]() const64
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::getNumPublishers() 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]() const436
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const16847
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const12936
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::getNumPublishers() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::hasMsg() const16849
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::peekMsg() const12937
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]() const392
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::getNumPublishers() 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() const17792
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const4056
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::getNumPublishers() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::hasMsg() const17792
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::peekMsg() const4056
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]() const226
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const436198
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const210372
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::getNumPublishers() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::hasMsg() const435979
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::peekMsg() const210380
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]() const704
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const1026535
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const797316
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]() const146
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::getNumPublishers() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::hasMsg() const1026427
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::peekMsg() const797369
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]() const1448
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const215
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const19629
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const308
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() const215
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::hasMsg() const19629
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::peekMsg() const308
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]() const500
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const185787
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const185781
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::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::hasMsg() const185787
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::peekMsg() const185781
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]() const444
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const464524
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const119815
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::newMsg() const563023
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const444023
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() const464346
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::hasMsg() const119815
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::newMsg() const562270
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::peekMsg() const444233
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]() const842
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const74204
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const50694
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::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::hasMsg() const74154
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::peekMsg() const50694
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]() const872
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const7
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() const785
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() const7
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::getNumPublishers() 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() const785
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]() const436
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::getNumPublishers() 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]() const232
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const785
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const785
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::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::hasMsg() const785
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::peekMsg() const785
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]() const218
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const276
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const2760
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const380
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() const276
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::hasMsg() const2760
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::peekMsg() const380
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]() const218
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::getNumPublishers() 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]() const20
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::getNumPublishers() 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]() const218
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const39903
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const20520
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]() const111
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::hasMsg() const39903
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::peekMsg() const20521
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]() const829
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::getNumPublishers() 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]() const218
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::getNumPublishers() 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]() const218
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::getNumPublishers() 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]() const218
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const110
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::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const110
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]() const218
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const88585
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const75288
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::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const88625
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const75288
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]() const936
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::getNumPublishers() 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]() const20
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const19396
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::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const19396
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]() const64
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const105
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() const105
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::getNumPublishers() 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::GainManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const218
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::getNumPublishers() 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]() const64
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::getNumPublishers() 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]() const218
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const154660
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const152278
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]() const122
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const154675
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const152281
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]() const1058
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const105
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() const105
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::getNumPublishers() 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::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const218
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::getNumPublishers() 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]() const218
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const104
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const104
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::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::hasMsg() const104
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::peekMsg() const104
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]() const18
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const232848
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const271724
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const4
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() const232848
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::hasMsg() const271726
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::peekMsg() const4
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]() const654
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const588089
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const250064
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]() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::getNumPublishers() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::hasMsg() const588095
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::peekMsg() const250064
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]() const674
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const40939
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const58949
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const40939
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() const40939
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::getNumPublishers() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::hasMsg() const58949
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::peekMsg() const40939
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]() const436
+
+
+ + + +
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..4846d28cc1 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.html @@ -0,0 +1,446 @@ + + + + + + + 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:9713174.0 %
Date:2024-12-17 22:35:29Functions:500114743.6 %
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        7129 :     Impl(const SubscribeHandlerOptions& options, const message_callback_t& message_callback = message_callback_t())
+      29        7129 :         : m_nh(options.nh),
+      30        7129 :           m_topic_name(options.topic_name),
+      31        7129 :           m_node_name(options.node_name),
+      32             :           m_got_data(false),
+      33             :           m_new_data(false),
+      34             :           m_used_data(false),
+      35        7129 :           m_timeout_manager(options.timeout_manager),
+      36             :           m_latest_message_time(0),
+      37             :           m_latest_message(nullptr),
+      38             :           m_message_callback(message_callback),
+      39        7129 :           m_queue_size(options.queue_size),
+      40        7129 :           m_transport_hints(options.transport_hints)
+      41             :     {
+      42             :       // initialize the callback for the TimeoutManager
+      43        7129 :       if (options.timeout_callback)
+      44           1 :         m_timeout_mgr_callback = std::bind(options.timeout_callback, topicName(), std::placeholders::_1);
+      45             :       else
+      46        7128 :         m_timeout_mgr_callback = std::bind(&Impl::default_timeout_callback, this, topicName(), std::placeholders::_1);
+      47             : 
+      48        7129 :       if (options.no_message_timeout != mrs_lib::no_timeout)
+      49             :       {
+      50             :         // initialize a new TimeoutManager if not provided by the user
+      51          85 :         if (!m_timeout_manager)
+      52          85 :           m_timeout_manager = std::make_shared<mrs_lib::TimeoutManager>(m_nh, ros::Rate(options.no_message_timeout * 0.5));
+      53             : 
+      54             :         // register the timeout callback with the TimeoutManager
+      55          85 :         m_timeout_id = m_timeout_manager->registerNew(options.no_message_timeout, m_timeout_mgr_callback);
+      56             :       }
+      57             : 
+      58       21387 :       const std::string msg = "Subscribed to topic '" + m_topic_name + "' -> '" + topicName() + "'";
+      59        7129 :       if (m_node_name.empty())
+      60           1 :         ROS_INFO_STREAM(msg);
+      61             :       else
+      62        7128 :         ROS_INFO_STREAM("[" << m_node_name << "]: " << msg);
+      63        7129 :     }
+      64             :     //}
+      65             : 
+      66        7129 :     virtual ~Impl() = default;
+      67             : 
+      68             :   public:
+      69             :     /* getMsg() method //{ */
+      70     2257000 :     virtual typename MessageType::ConstPtr getMsg()
+      71             :     {
+      72     2257000 :       m_new_data = false;
+      73     2257000 :       m_used_data = true;
+      74     2257000 :       return peekMsg();
+      75             :     }
+      76             :     //}
+      77             : 
+      78             :     /* peekMsg() method //{ */
+      79     2257156 :     virtual typename MessageType::ConstPtr peekMsg() const
+      80             :     {
+      81             :       /* assert(m_got_data); */
+      82             :       /* if (!m_got_data) */
+      83             :       /*   ROS_ERROR("[%s]: No data received yet from topic '%s' (forgot to check hasMsg()?)! Returning empty message.", m_node_name.c_str(), */
+      84             :       /*             topicName().c_str()); */
+      85     2257156 :       return m_latest_message;
+      86             :     }
+      87             :     //}
+      88             : 
+      89             :     /* hasMsg() method //{ */
+      90     3131802 :     virtual bool hasMsg() const
+      91             :     {
+      92     3131802 :       return m_got_data;
+      93             :     }
+      94             :     //}
+      95             : 
+      96             :     /* newMsg() method //{ */
+      97      562270 :     virtual bool newMsg() const
+      98             :     {
+      99      562270 :       return m_new_data;
+     100             :     }
+     101             :     //}
+     102             : 
+     103             :     /* usedMsg() method //{ */
+     104           0 :     virtual bool usedMsg() const
+     105             :     {
+     106           0 :       return m_used_data;
+     107             :     }
+     108             :     //}
+     109             : 
+     110             :     /* waitForNew() method //{ */
+     111           0 :     virtual typename MessageType::ConstPtr waitForNew(const ros::WallDuration& timeout)
+     112             :     {
+     113             :       // convert the ros type to chrono type
+     114           0 :       const std::chrono::duration<float> chrono_timeout(timeout.toSec());
+     115             :       // lock the mutex guarding the m_new_data flag
+     116           0 :       std::unique_lock lock(m_new_data_mtx);
+     117             :       // if new data is available, return immediately, otherwise attempt to wait for new data using the respective condition variable
+     118           0 :       if (m_new_data)
+     119           0 :         return getMsg();
+     120           0 :       else if (m_new_data_cv.wait_for(lock, chrono_timeout) == std::cv_status::no_timeout && m_new_data)
+     121           0 :         return getMsg();
+     122             :       else
+     123           0 :         return nullptr;
+     124             :     };
+     125             :     //}
+     126             : 
+     127             :     /* lastMsgTime() method //{ */
+     128      738846 :     virtual ros::Time lastMsgTime() const
+     129             :     {
+     130      738846 :       return m_latest_message_time;
+     131             :     };
+     132             :     //}
+     133             : 
+     134             :     /* topicName() method //{ */
+     135       14639 :     virtual std::string topicName() const
+     136             :     {
+     137       14639 :       std::string ret = m_sub.getTopic();
+     138       14639 :       if (ret.empty())
+     139       14258 :         ret = m_nh.resolveName(m_topic_name);
+     140       14639 :       return ret;
+     141             :     }
+     142             :     //}
+     143             : 
+     144             :     /* getNumPublishers() method //{ */
+     145           0 :     virtual uint32_t getNumPublishers() const
+     146             :     {
+     147           0 :       return m_sub.getNumPublishers();
+     148             :     };
+     149             :     //}
+     150             : 
+     151             :     /* setNoMessageTimeout() method //{ */
+     152           0 :     virtual void setNoMessageTimeout(const ros::Duration& timeout)
+     153             :     {
+     154           0 :       if (timeout == mrs_lib::no_timeout)
+     155             :       {
+     156             :         // if there is a timeout callback already registered but the user wants to disable it, pause it
+     157           0 :         if (m_timeout_manager != nullptr && m_timeout_id.has_value())
+     158           0 :           m_timeout_manager->pause(m_timeout_id.value());
+     159             :         // otherwise, there is no callback, so nothing to do
+     160             :       }
+     161             :       else
+     162             :       {
+     163             :         // if there is no callback manager, create it
+     164           0 :         if (m_timeout_manager == nullptr)
+     165           0 :           m_timeout_manager = std::make_shared<mrs_lib::TimeoutManager>(m_nh, ros::Rate(timeout * 0.5));
+     166             : 
+     167             :         // if there is an existing timeout callback registered, change its timeout
+     168           0 :         if (m_timeout_id.has_value())
+     169           0 :           m_timeout_manager->change(m_timeout_id.value(), timeout, m_timeout_mgr_callback);
+     170             :         // otherwise, register it
+     171             :         else
+     172           0 :           m_timeout_id = m_timeout_manager->registerNew(timeout, m_timeout_mgr_callback);
+     173             :       }
+     174           0 :     }
+     175             :     //}
+     176             : 
+     177             :     /* start() method //{ */
+     178        7133 :     virtual void start()
+     179             :     {
+     180        7133 :       if (m_timeout_manager && m_timeout_id.has_value())
+     181          89 :         m_timeout_manager->start(m_timeout_id.value());
+     182        7133 :       m_sub = m_nh.subscribe(m_topic_name, m_queue_size, &Impl::data_callback, this, m_transport_hints);
+     183        7133 :     }
+     184             :     //}
+     185             : 
+     186             :     /* stop() method //{ */
+     187           5 :     virtual void stop()
+     188             :     {
+     189           5 :       if (m_timeout_manager && m_timeout_id.has_value())
+     190           5 :         m_timeout_manager->pause(m_timeout_id.value());
+     191           5 :       m_sub.shutdown();
+     192           5 :     }
+     193             :     //}
+     194             : 
+     195             :   protected:
+     196             :     ros::NodeHandle m_nh;
+     197             :     ros::Subscriber m_sub;
+     198             : 
+     199             :   protected:
+     200             :     std::string m_topic_name;
+     201             :     std::string m_node_name;
+     202             : 
+     203             :   protected:
+     204             :     bool m_got_data;   // whether any data was received
+     205             : 
+     206             :     mutable std::mutex m_new_data_mtx;
+     207             :     mutable std::condition_variable m_new_data_cv;
+     208             :     bool m_new_data;   // whether new data was received since last call to get_data
+     209             : 
+     210             :     bool m_used_data;  // whether get_data was successfully called at least once
+     211             : 
+     212             :   protected:
+     213             :     std::shared_ptr<mrs_lib::TimeoutManager> m_timeout_manager;
+     214             :     std::optional<mrs_lib::TimeoutManager::timeout_id_t> m_timeout_id;
+     215             :     mrs_lib::TimeoutManager::callback_t m_timeout_mgr_callback;
+     216             : 
+     217             :   protected:
+     218             :     ros::Time m_latest_message_time;
+     219             :     typename MessageType::ConstPtr m_latest_message;
+     220             :     message_callback_t m_message_callback;
+     221             : 
+     222             :   private:
+     223             :     uint32_t m_queue_size;
+     224             :     ros::TransportHints m_transport_hints;
+     225             : 
+     226             :   protected:
+     227             :     /* default_timeout_callback() method //{ */
+     228           2 :     void default_timeout_callback(const std::string& topic_name, const ros::Time& last_msg)
+     229             :     {
+     230           2 :       const ros::Duration since_msg = (ros::Time::now() - last_msg);
+     231           2 :       const auto n_pubs = m_sub.getNumPublishers();
+     232           6 :       const std::string txt = "Did not receive any message from topic '" + topic_name + "' for " + std::to_string(since_msg.toSec()) + "s ("
+     233             :                               + std::to_string(n_pubs) + " publishers on this topic)";
+     234           2 :       if (m_node_name.empty())
+     235           0 :         ROS_WARN_STREAM(txt);
+     236             :       else
+     237           2 :         ROS_WARN_STREAM("[" << m_node_name << "]: " << txt);
+     238           2 :     }
+     239             :     //}
+     240             : 
+     241             :     /* process_new_message() method //{ */
+     242     6763146 :     void process_new_message(const typename MessageType::ConstPtr& msg)
+     243             :     {
+     244     6763146 :       m_latest_message_time = ros::Time::now();
+     245     6762570 :       m_latest_message = msg;
+     246             :       // If the message callback is registered, the new data will immediately be processed,
+     247             :       // so reset the flag. Otherwise, set the flag.
+     248     6764729 :       m_new_data = !m_message_callback;
+     249     6760617 :       m_got_data = true;
+     250     6760617 :       m_new_data_cv.notify_one();
+     251     6767899 :     }
+     252             :     //}
+     253             : 
+     254             :     /* data_callback() method //{ */
+     255           0 :     virtual void data_callback(const typename MessageType::ConstPtr& msg)
+     256             :     {
+     257             :       {
+     258           0 :         std::lock_guard lck(m_new_data_mtx);
+     259           0 :         if (m_timeout_manager && m_timeout_id.has_value())
+     260           0 :           m_timeout_manager->reset(m_timeout_id.value());
+     261           0 :         process_new_message(msg);
+     262             :       }
+     263             : 
+     264             :       // execute the callback after unlocking the mutex to enable multi-threaded callback execution
+     265           0 :       if (m_message_callback)
+     266           0 :         m_message_callback(msg);
+     267           0 :     }
+     268             :     //}
+     269             :   };
+     270             :   //}
+     271             : 
+     272             :   /* SubscribeHandler_threadsafe class //{ */
+     273             :   template <typename MessageType>
+     274             :   class SubscribeHandler<MessageType>::ImplThreadsafe : public SubscribeHandler<MessageType>::Impl
+     275             :   {
+     276             :   private:
+     277             :     using impl_class_t = SubscribeHandler<MessageType>::Impl;
+     278             : 
+     279             :   public:
+     280             :     using timeout_callback_t = typename impl_class_t::timeout_callback_t;
+     281             :     using message_callback_t = typename impl_class_t::message_callback_t;
+     282             : 
+     283             :     friend class SubscribeHandler<MessageType>;
+     284             : 
+     285             :   public:
+     286        7129 :     ImplThreadsafe(const SubscribeHandlerOptions& options, const message_callback_t& message_callback = message_callback_t())
+     287        7129 :         : impl_class_t::Impl(options, message_callback)
+     288             :     {
+     289        7129 :     }
+     290             : 
+     291             :   public:
+     292     3132114 :     virtual bool hasMsg() const override
+     293             :     {
+     294     6263897 :       std::lock_guard lck(m_mtx);
+     295     6264269 :       return impl_class_t::hasMsg();
+     296             :     }
+     297      563023 :     virtual bool newMsg() const override
+     298             :     {
+     299     1124410 :       std::lock_guard lck(m_mtx);
+     300     1124699 :       return impl_class_t::newMsg();
+     301             :     }
+     302           0 :     virtual bool usedMsg() const override
+     303             :     {
+     304           0 :       std::lock_guard lck(m_mtx);
+     305           0 :       return impl_class_t::usedMsg();
+     306             :     }
+     307     2256735 :     virtual typename MessageType::ConstPtr getMsg() override
+     308             :     {
+     309     4513714 :       std::lock_guard lck(m_mtx);
+     310     4513749 :       return impl_class_t::getMsg();
+     311             :     }
+     312     2256880 :     virtual typename MessageType::ConstPtr peekMsg() const override
+     313             :     {
+     314     4513688 :       std::lock_guard lck(m_mtx);
+     315     4513618 :       return impl_class_t::peekMsg();
+     316             :     }
+     317      739024 :     virtual ros::Time lastMsgTime() const override
+     318             :     {
+     319     1477937 :       std::lock_guard lck(m_mtx);
+     320     1477928 :       return impl_class_t::lastMsgTime();
+     321             :     };
+     322         379 :     virtual std::string topicName() const override
+     323             :     {
+     324         760 :       std::lock_guard lck(m_mtx);
+     325         762 :       return impl_class_t::topicName();
+     326             :     };
+     327        7133 :     virtual void start() override
+     328             :     {
+     329       14266 :       std::lock_guard lck(m_mtx);
+     330       14266 :       return impl_class_t::start();
+     331             :     }
+     332           5 :     virtual void stop() override
+     333             :     {
+     334          10 :       std::lock_guard lck(m_mtx);
+     335          10 :       return impl_class_t::stop();
+     336             :     }
+     337             : 
+     338       14258 :     virtual ~ImplThreadsafe() override = default;
+     339             : 
+     340             :   protected:
+     341     6764148 :     virtual void data_callback(const typename MessageType::ConstPtr& msg) override
+     342             :     {
+     343             :       {
+     344    13531126 :         std::scoped_lock lck(m_mtx, this->m_new_data_mtx);
+     345     6744447 :         if (this->m_timeout_manager && this->m_timeout_id.has_value())
+     346      143600 :           this->m_timeout_manager->reset(this->m_timeout_id.value());
+     347     6732384 :         impl_class_t::process_new_message(msg);
+     348             :       }
+     349             : 
+     350             :       // execute the callback after unlocking the mutex to enable multi-threaded callback execution
+     351     6746338 :       if (this->m_message_callback)
+     352     3724809 :         impl_class_t::m_message_callback(msg);
+     353     6740706 :     }
+     354             : 
+     355             :   private:
+     356             :     mutable std::recursive_mutex m_mtx;
+     357             :   };
+     358             :   //}
+     359             : 
+     360             : }  // namespace mrs_lib
+     361             : 
+     362             : #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..c387463802 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.overview.html @@ -0,0 +1,111 @@ + + + + + + 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 + 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..70801f86e8741bd2798cde7ee5fdf5843d6796c3 GIT binary patch literal 1519 zcmVF##mn$3#|4LJ*w>h;0UPBU|^h zB$h_5W=Pv4S*mpna6%yX8L1>e7$JC|LcIJPe^cnuL?KQ0fq7oeWtXyC{Gxn!d^!N< z*=3NKT^?{=$Z9;$1Z*KDfF06FMKX&@4AK;)!D$W1sECjr@mV``&FotWZ|U>^>=>8` zgQAZqQD>q&A|GSbLs+zSp(}jjF?M`GgK_ab{}72BF=AmB>&j;gC{pADC^E?$1l?UnQq<6WQ07-)Jddca3QIr;b$(~>WtJv9@f zX~YAnvla)ygrlwSZ4=7(j16zS^r6}_wy&H2>iG-QGY$Z+*S_!DzOTpI`SN1c0l0C$ zQNY(8@$J1w zoIf%HIQDOubje+0L7o6cpA7@Dj2QE%Xmkq!HG_blo_LgrdxAXQQV(aDqMmJ|8p;SB z67y9i%siIndY(lP9y#9`nGiTJHsNWuE{_t^0MaY1paA7v?%|5fHK>fBFk;`46!z%g zb5B;7VS~-#b}0pH_oPYsioHFO;w*aeAHUf*3kn|}G@-(8Y7EVvQ8-_mT;lO|h2z%q z5HgHXrGPh0*_Aw|K9>~m$9W9Z$HRm?J*83e zOwxIH`53TNg?!l>i#TBiX`X-o7*KrDth~n-|B3&1T4CVWmG6>3K-+6;1Y5f>)r`81 zo)nxf{WpjmZ(N~sC>1@C24n6feFP|%nnO4L8mH(VEb8{dy5lbOnwrw`ez+_fu_Ka8fn#Z|Z z09?6I86KH%aih6WDXvn|%}`U5my#o-Z@bJnDFHQ1e&^~X4B@?NmH0+w0jc7?CfLif zs|-*lH5i?9<RVt88$IFR+TjW9bxFyzS9|24M|FJ?WMRKk002EuaGo<=?kms zudqz_LKV2l_9McyL9Yk{hR6*q{fIE^I8``pZjOAvCJc9|PzqxgJq!CfP~%?R^x2HXc|KgX_b3IL!yW(m73IA^yWEG&(d2C zVB~%!q;Ef83r}%WXzAZnG&Gn2KT$u*o2j7aH*DScy>nsc{mcH&Pqt3wz%vR5_Xi6z VixoZDtP21D002ovPDHLkV1oL?!xjJl literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/impl/timer.hpp.func-sort-c.html b/mrs_lib/include/mrs_lib/impl/timer.hpp.func-sort-c.html new file mode 100644 index 0000000000..af74da8486 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/timer.hpp.func-sort-c.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-12-17 22:35:29Functions: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..c27cb333eb --- /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-12-17 22:35:29Functions: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..5894a7f475 --- /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-12-17 22:35:29Functions: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-12-17 22:35:29Functions: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&)92
void mrs_lib::Transformer::setHeader<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)92
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&)809
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&)22449
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&)22449
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)22449
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&)26714
void mrs_lib::Transformer::setHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)26714
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&)103458
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&)108409
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&)201470
void mrs_lib::Transformer::setHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)201470
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&)251250
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&)277822
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&)277849
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&)277963
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> > const&)304644
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&)585771
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&)585961
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&)635485
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&)787381
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&)831843
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&)831851
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&)832033
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&)832664
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&)832755
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&)836956
+
+
+ + + +
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..c18847ff01 --- /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-12-17 22:35:29Functions: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&)635485
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&)251250
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&)832664
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&)836956
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&)277963
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&)832755
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&)201470
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&)26714
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&)92
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&)585961
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&)585771
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&)277822
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&)277849
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&)831851
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&)831843
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&)22449
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&)22449
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&)787381
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> > const&)304644
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&)832033
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)22449
void mrs_lib::Transformer::setHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)201470
void mrs_lib::Transformer::setHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)26714
void mrs_lib::Transformer::setHeader<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)92
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&)108409
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&)809
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&)103458
+
+
+ + + +
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..20f4e39fd2 --- /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-12-17 22:35:29Functions: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     1946507 :   std_msgs::Header Transformer::getHeader(const msg_t& msg)
+      15             :   {
+      16     1946507 :     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      228276 :   void Transformer::setHeader(msg_t& msg, const std_msgs::Header& header)
+      33             :   {
+      34      228276 :     msg.header = header;
+      35      228276 :   }
+      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      228276 :   T Transformer::copyChangeFrame(const T& what, const std::string& frame_id)
+      49             :   {
+      50      228276 :     T ret = what;
+      51             :     if constexpr (has_header_member_v<T>)
+      52             :     {
+      53      456552 :       std_msgs::Header new_header = getHeader(what);
+      54      228276 :       new_header.frame_id = frame_id;
+      55      228276 :       setHeader(ret, new_header);
+      56             :     }
+      57      228276 :     return ret;
+      58             :   }
+      59             : 
+      60             :   //}
+      61             : 
+      62             :   /* transformImpl() //{ */
+      63             : 
+      64             :   template <class T>
+      65     1947717 :   std::optional<T> Transformer::transformImpl(const geometry_msgs::TransformStamped& tf, const T& what)
+      66             :   {
+      67     3895426 :     const std::string from_frame = frame_from(tf);
+      68     3895397 :     const std::string to_frame = frame_to(tf);
+      69             : 
+      70     1947710 :     if (from_frame == to_frame)
+      71      228276 :       return copyChangeFrame(what, from_frame);
+      72             : 
+      73     3438874 :     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      886749 :       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      886747 :       else if (to_frame == latlon_frame_name)
+      89             :       {
+      90        4382 :         const std::optional<T> tmp = doTransform(what, tf);
+      91        2192 :         if (!tmp.has_value())
+      92           0 :           return std::nullopt;
+      93        2192 :         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      832691 :       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     1717246 :     return doTransform(what, tf);
+     108             :   }
+     109             : 
+     110             :   //}
+     111             : 
+     112             :   /* doTransform() //{ */
+     113             : 
+     114             :   template <class T>
+     115     1719440 :   std::optional<T> Transformer::doTransform(const T& what, const geometry_msgs::TransformStamped& tf)
+     116             :   {
+     117             :     try
+     118             :     {
+     119     3438800 :       T result;
+     120     1719421 :       tf2::doTransform(what, result, tf);
+     121     1719431 :       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     1717912 :   std::optional<T> Transformer::transformSingle(const T& what, const std::string& to_frame_raw)
+     139             :   {
+     140     3436531 :     const std_msgs::Header orig_header = getHeader(what);
+     141     3436322 :     return transformSingle(orig_header.frame_id, what, to_frame_raw, orig_header.stamp);
+     142             :   }
+     143             : 
+     144             :   template <class T>
+     145     1718084 :   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     3436718 :     std::scoped_lock lck(mutex_);
+     148             : 
+     149     1718641 :     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     3437255 :     const std::string from_frame = resolveFrameImpl(from_frame_raw);
+     156     3437238 :     const std::string to_frame = resolveFrameImpl(to_frame_raw);
+     157     3437240 :     const std::string latlon_frame = resolveFrameImpl(LATLON_ORIGIN);
+     158             : 
+     159             :     // get the transform
+     160     3437269 :     const auto tf_opt = getTransformImpl(from_frame, to_frame, time_stamp, latlon_frame);
+     161     1718645 :     if (!tf_opt.has_value())
+     162       11014 :       return std::nullopt;
+     163     1707625 :     const geometry_msgs::TransformStamped& tf = tf_opt.value();
+     164             : 
+     165             :     // do the transformation
+     166     3415209 :     const geometry_msgs::TransformStamped tf_resolved = create_transform(from_frame, to_frame, tf.header.stamp, tf.transform);
+     167     1707627 :     return transformImpl(tf_resolved, what);
+     168             :   }
+     169             : 
+     170             :   //}
+     171             : 
+     172             :   /* transform() //{ */
+     173             : 
+     174             :   template <class T>
+     175      212694 :   std::optional<T> Transformer::transform(const T& what, const geometry_msgs::TransformStamped& tf)
+     176             :   {
+     177      425388 :     std::scoped_lock lck(mutex_);
+     178             : 
+     179      212694 :     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      425388 :     const std::string from_frame = resolveFrameImpl(frame_from(tf));
+     186      425388 :     const std::string to_frame = resolveFrameImpl(frame_to(tf));
+     187      425388 :     const geometry_msgs::TransformStamped tf_resolved = create_transform(from_frame, to_frame, tf.header.stamp, tf.transform);
+     188             : 
+     189      212694 :     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..4aa58c90e3 --- /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-12-17 22:35:29Functions: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..1c9fecdac0 --- /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-12-17 22:35:29Functions: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..5784838823 --- /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-12-17 22:35:29Functions: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..593657a73e --- /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-12-17 22:35:29Functions: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..8304d68b57 --- /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-12-17 22:35:29Functions: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..3f396ee9f8 --- /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-12-17 22:35:29Functions: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:78198879.0 %
Date:2024-12-17 22:35:29Functions:853147557.8 %
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 +
86.3%86.3%
+ 		86.3 %44 / 5146.2 %398 / 862
<unnamed>86.3 %44 / 5146.2 %398 / 862
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 / 498.4 %120 / 122
<unnamed>100.0 %4 / 498.4 %120 / 122
timeout_manager.h +
80.0%80.0%
+ 		80.0 %4 / 5-0 / 0
<unnamed>80.0 %4 / 5-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..5790093177 --- /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:78198879.0 %
Date:2024-12-17 22:35:29Functions:853147557.8 %
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
timeout_manager.h +
80.0%80.0%
+ 		80.0 %4 / 5-0 / 0
<unnamed>80.0 %4 / 5-0 / 0
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 +
86.3%86.3%
+ 		86.3 %44 / 5146.2 %398 / 862
<unnamed>86.3 %44 / 5146.2 %398 / 862
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
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 / 498.4 %120 / 122
<unnamed>100.0 %4 / 498.4 %120 / 122
timer.h +
100.0%
+ 		100.0 %4 / 485.7 %6 / 7
<unnamed>100.0 %4 / 485.7 %6 / 7
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..99be8f46ca --- /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:78198879.0 %
Date:2024-12-17 22:35:29Functions:853147557.8 %
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 / 498.4 %120 / 122
<unnamed>100.0 %4 / 498.4 %120 / 122
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 +
86.3%86.3%
+ 		86.3 %44 / 5146.2 %398 / 862
<unnamed>86.3 %44 / 5146.2 %398 / 862
timeout_manager.h +
80.0%80.0%
+ 		80.0 %4 / 5-0 / 0
<unnamed>80.0 %4 / 5-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..52d0480c9d --- /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:78198879.0 %
Date:2024-12-17 22:35:29Functions:853147557.8 %
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 +
86.3%86.3%
+ 		86.3 %44 / 5146.2 %398 / 862
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 / 498.4 %120 / 122
timeout_manager.h +
80.0%80.0%
+ 		80.0 %4 / 5-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..1d4e2a998d --- /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:78198879.0 %
Date:2024-12-17 22:35:29Functions:853147557.8 %
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
timeout_manager.h +
80.0%80.0%
+ 		80.0 %4 / 5-0 / 0
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 +
86.3%86.3%
+ 		86.3 %44 / 5146.2 %398 / 862
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
quadratic_throttle_model.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
publisher_handler.h +
100.0%
+ 		100.0 %4 / 498.4 %120 / 122
timer.h +
100.0%
+ 		100.0 %4 / 485.7 %6 / 7
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..e1ea3783de --- /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:78198879.0 %
Date:2024-12-17 22:35:29Functions:853147557.8 %
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 / 498.4 %120 / 122
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 +
86.3%86.3%
+ 		86.3 %44 / 5146.2 %398 / 862
timeout_manager.h +
80.0%80.0%
+ 		80.0 %4 / 5-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..9ee1236adf --- /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-12-17 22:35:29Functions: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&)116
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&)196
mrs_lib::LKF<2, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)5842
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&) const5842
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&) const5842
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&) const5842
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&)18018
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)18018
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) const18018
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&)198454
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) const198454
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)198458
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&) const228767
mrs_lib::LKF<6, 2, 2>::invert_W(Eigen::Matrix<double, 2, 2, 0, 2, 2>)228962
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&) const229081
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&) const229197
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&)312402
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) const312554
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)312664
mrs_lib::LKF<3, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)485928
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&) const486897
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&) const487142
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&) const487165
+
+
+ + + +
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..fd63d35cd3 --- /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-12-17 22:35:29Functions: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&)18018
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)18018
mrs_lib::LKF<2, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)5842
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&)312402
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)312664
mrs_lib::LKF<3, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)485928
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&)196
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&)198454
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)198458
mrs_lib::LKF<6, 2, 2>::invert_W(Eigen::Matrix<double, 2, 2, 0, 2, 2>)228962
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&)116
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) const18018
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&) const5842
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&) const5842
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&) const5842
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&) const487165
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&) const487142
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&) const486897
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) const312554
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&) const229081
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&) const229197
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&) const228767
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) const198454
+
+
+ + + +
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..61a160ef2b --- /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-12-17 22:35:29Functions: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         312 :     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      721506 :     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      721506 :       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      511008 :     virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, [[maybe_unused]] double dt) const override
+     139             :     {
+     140      511008 :       statecov_t ret;
+     141      509962 :       ret.x = state_predict(A, sc.x, B, u);
+     142      510346 :       ret.P = covariance_predict(A, sc.P, Q, dt);
+     143      510170 :       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      529140 :     static P_t covariance_predict(const A_t& A, const P_t& P, const Q_t& Q, const double dt)
+     155             :     {
+     156      529140 :       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      528874 :     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      528874 :       return A * x + B * u;
+     172             :     }
+     173             :     //}
+     174             : 
+     175             :   protected:
+     176             :     /* invert_W() method //{ */
+     177      720732 :     static R_t invert_W(R_t W)
+     178             :     {
+     179      720732 :       Eigen::ColPivHouseholderQR<R_t> qr(W);
+     180      721998 :       if (!qr.isInvertible())
+     181             :       {
+     182             :         // add some stuff to the tmp matrix diagonal to make it invertible
+     183       53633 :         R_t ident = R_t::Identity(W.rows(), W.cols());
+     184       53607 :         W += 1e-9 * ident;
+     185       53649 :         qr.compute(W);
+     186       53677 :         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      718901 :       const R_t W_inv = qr.inverse();
+     193     1443702 :       return W_inv;
+     194             :     }
+     195             :     //}
+     196             : 
+     197             :     /* computeKalmanGain() method //{ */
+     198      722181 :     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      722181 :       const R_t W = H * sc.P * H.transpose() + R;
+     202      721696 :       const R_t W_inv = invert_W(W);
+     203      722014 :       const K_t K = sc.P * H.transpose() * W_inv;
+     204     1442500 :       return K;
+     205             :     }
+     206             :     //}
+     207             : 
+     208             :     /* correction_impl() method //{ */
+     209             :     template<int check=n>
+     210      722088 :     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      722088 :       statecov_t ret;
+     214      721414 :       const K_t K = computeKalmanGain(sc, z, R, H);
+     215      721522 :       ret.x = sc.x + K * (z - (H * sc.x));
+     216      721827 :       ret.P = (P_t::Identity() - (K * H)) * sc.P;
+     217     1443196 :       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       18018 :     virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, double dt) const override
+     317             :     {
+     318       18018 :       statecov_t ret;
+     319       18018 :       A_t A = m_generateA(dt);
+     320       18018 :       B_t B = m_generateB(dt);
+     321       18018 :       ret.x = Base_class::state_predict(A, sc.x, B, u);
+     322       18018 :       ret.P = Base_class::covariance_predict(A, sc.P, Q, dt);
+     323       36036 :       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..0ff4ecc7a4 --- /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-12-17 22:35:29Functions: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&)344
mrs_lib::getHeading(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)344
mrs_lib::getPosition(mrs_msgs::Reference_<std::allocator<void> > const&)344
mrs_lib::getPosition(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)344
mrs_lib::getPosition(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)344
mrs_lib::getPose(nav_msgs::Odometry_<std::allocator<void> > const&)1851
mrs_lib::getHeading(geometry_msgs::Pose_<std::allocator<void> > const&)247343
mrs_lib::getHeading(geometry_msgs::PoseWithCovariance_<std::allocator<void> > const&)247344
mrs_lib::getHeading(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)247344
mrs_lib::getHeading(nav_msgs::Odometry_<std::allocator<void> > const&)247344
mrs_lib::getPosition(geometry_msgs::PoseWithCovariance_<std::allocator<void> > const&)247345
mrs_lib::getPosition(nav_msgs::Odometry_<std::allocator<void> > const&)247345
mrs_lib::getPosition(geometry_msgs::Pose_<std::allocator<void> > const&)247346
mrs_lib::getPosition(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)247346
mrs_lib::getXYZ(geometry_msgs::Point_<std::allocator<void> > const&)248034
+
+
+ + + +
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..38c803f19b --- /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-12-17 22:35:29Functions: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&)247344
mrs_lib::getHeading(geometry_msgs::Pose_<std::allocator<void> > const&)247343
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&)247344
mrs_lib::getHeading(mrs_msgs::Reference_<std::allocator<void> > const&)344
mrs_lib::getHeading(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)0
mrs_lib::getHeading(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)344
mrs_lib::getHeading(nav_msgs::Odometry_<std::allocator<void> > const&)247344
mrs_lib::getPosition(geometry_msgs::PoseWithCovariance_<std::allocator<void> > const&)247345
mrs_lib::getPosition(geometry_msgs::Pose_<std::allocator<void> > const&)247346
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&)247346
mrs_lib::getPosition(mrs_msgs::Reference_<std::allocator<void> > const&)344
mrs_lib::getPosition(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)344
mrs_lib::getPosition(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)344
mrs_lib::getPosition(nav_msgs::Odometry_<std::allocator<void> > const&)247345
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&)248034
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&)1851
+
+
+ + + +
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..220f0f6c3f --- /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-12-17 22:35:29Functions: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      248034 : std::tuple<double, double, double> getXYZ(const geometry_msgs::Point& data) {
+      30             : 
+      31      248034 :   double x = data.x;
+      32      248034 :   double y = data.y;
+      33      248034 :   double z = data.z;
+      34             : 
+      35      496061 :   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      247346 : std::tuple<double, double, double> getPosition(const geometry_msgs::Pose& data) {
+      96             : 
+      97      247346 :   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      247343 : double getHeading(const geometry_msgs::Pose& data) {
+     124             : 
+     125      247343 :   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      247345 : std::tuple<double, double, double> getPosition(const geometry_msgs::PoseWithCovariance& data) {
+     184             : 
+     185      247345 :   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      247344 : double getHeading(const geometry_msgs::PoseWithCovariance& data) {
+     212             : 
+     213      247344 :   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      247345 : std::tuple<double, double, double> getPosition(const nav_msgs::Odometry& data) {
+     336             : 
+     337      247345 :   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      247346 : std::tuple<double, double, double> getPosition(const nav_msgs::OdometryConstPtr& data) {
+     348             : 
+     349      247346 :   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      247344 : double getHeading(const nav_msgs::Odometry& data) {
+     392             : 
+     393      247344 :   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      247344 : double getHeading(const nav_msgs::OdometryConstPtr& data) {
+     404             : 
+     405      247344 :   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        1851 : geometry_msgs::Pose getPose(const nav_msgs::Odometry& data) {
+     448             : 
+     449        1851 :   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         344 : std::tuple<double, double, double> getPosition(const mrs_msgs::TrackerCommand& data) {
+     480             : 
+     481         344 :   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         344 : std::tuple<double, double, double> getPosition(const mrs_msgs::Reference& data) {
+     579             : 
+     580         344 :   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         344 : double getHeading(const mrs_msgs::Reference& data) {
+     607             : 
+     608         344 :   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         344 : std::tuple<double, double, double> getPosition(const mrs_msgs::ReferenceStamped& data) {
+     639             : 
+     640         344 :   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         344 : double getHeading(const mrs_msgs::ReferenceStamped& data) {
+     667             : 
+     668         344 :   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..6a55ed7534 --- /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-12-17 22:35:29Functions: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&)109
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> >&)166
auto mrs_lib::set_mutexed<mrs_uav_controllers::mpc_controllerConfig>(std::mutex&, mrs_uav_controllers::mpc_controllerConfig, mrs_uav_controllers::mpc_controllerConfig&)218
auto mrs_lib::set_mutexed<mrs_uav_controllers::se3_controllerConfig>(std::mutex&, mrs_uav_controllers::se3_controllerConfig, mrs_uav_controllers::se3_controllerConfig&)219
mrs_msgs::ObstacleSectors_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ObstacleSectors_<std::allocator<void> > >(std::mutex&, mrs_msgs::ObstacleSectors_<std::allocator<void> >&)435
mrs_msgs::VelocityReference_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::VelocityReference_<std::allocator<void> > >(std::mutex&, mrs_msgs::VelocityReference_<std::allocator<void> >&)818
mrs_msgs::ReferenceStamped_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(std::mutex&, mrs_msgs::ReferenceStamped_<std::allocator<void> >&)1113
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> >&)2498
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&)4684
int mrs_lib::get_mutexed<int>(std::mutex&, int&)9441
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&)13290
std::tuple<int, double> mrs_lib::get_mutexed<int, double>(std::mutex&, int&, double&)15783
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&)16599
std::tuple<double, double> mrs_lib::get_mutexed<double, double>(std::mutex&, double&, double&)17245
std::optional<double> mrs_lib::get_mutexed<std::optional<double> >(std::mutex&, std::optional<double>&)19436
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&)20010
std::tuple<bool, double, double> mrs_lib::get_mutexed<bool, double, double>(std::mutex&, bool&, double&, double&)22043
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&)23912
std::tuple<double, double, double> mrs_lib::get_mutexed<double, double, double>(std::mutex&, double&, double&, double&)28498
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&)32942
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> >&)38271
mrs_uav_controllers::se3_controllerConfig mrs_lib::get_mutexed<mrs_uav_controllers::se3_controllerConfig>(std::mutex&, mrs_uav_controllers::se3_controllerConfig&)58289
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&)81105
mrs_msgs::MpcPredictionFullState_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::MpcPredictionFullState_<std::allocator<void> > >(std::mutex&, mrs_msgs::MpcPredictionFullState_<std::allocator<void> >&)81217
mrs_uav_trackers::mpc_trackerConfig mrs_lib::get_mutexed<mrs_uav_trackers::mpc_trackerConfig>(std::mutex&, mrs_uav_trackers::mpc_trackerConfig&)84480
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&)88415
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> >&)91325
auto mrs_lib::set_mutexed<mrs_uav_managers::Controller::ControlOutput>(std::mutex&, mrs_uav_managers::Controller::ControlOutput, mrs_uav_managers::Controller::ControlOutput&)111575
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> >&)129683
mrs_uav_controllers::mpc_controllerConfig mrs_lib::get_mutexed<mrs_uav_controllers::mpc_controllerConfig>(std::mutex&, mrs_uav_controllers::mpc_controllerConfig&)161101
mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&)168673
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>&)198426
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&)210220
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&)211554
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&)218240
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> >&)231587
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>&)232637
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> >&)252073
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>&)257810
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&)261359
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>&)272168
mrs_msgs::DynamicsConstraints_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraints_<std::allocator<void> >&)292162
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>&)312035
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&)351996
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&)352554
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&)387831
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> >&)423085
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&)427523
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&)427593
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> >&)552749
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&)638699
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> > >&)735867
double const mrs_lib::get_mutexed<double const>(std::mutex&, double const&)764416
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&)799064
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&)799966
mrs_uav_managers::Controller::ControlOutput mrs_lib::get_mutexed<mrs_uav_managers::Controller::ControlOutput>(std::mutex&, mrs_uav_managers::Controller::ControlOutput&)841949
ros::Time mrs_lib::get_mutexed<ros::Time>(std::mutex&, ros::Time&)952846
unsigned int mrs_lib::get_mutexed<unsigned int>(std::mutex&, unsigned int&)1047136
mrs_msgs::UavState_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::UavState_<std::allocator<void> > >(std::mutex&, mrs_msgs::UavState_<std::allocator<void> >&)1074136
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> >&)1103063
auto mrs_lib::set_mutexed<double>(std::mutex&, double, double&)1248651
double mrs_lib::get_mutexed<double>(std::mutex&, double&)1811186
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&)2469692
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&)2742591
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&)2942935
+
+
+ + + +
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..45c3795d20 --- /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-12-17 22:35:29Functions: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&)81105
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>&)257810
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&)351996
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>&)232637
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&)13290
std::tuple<bool, double, double> mrs_lib::get_mutexed<bool, double, double>(std::mutex&, bool&, double&, double&)22043
std::tuple<double, double> mrs_lib::get_mutexed<double, double>(std::mutex&, double&, double&)17245
std::tuple<double, double, double> mrs_lib::get_mutexed<double, double, double>(std::mutex&, double&, double&, double&)28498
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&)23912
std::tuple<int, double> mrs_lib::get_mutexed<int, double>(std::mutex&, int&, double&)15783
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&)2742591
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&)210220
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&)638699
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&)218240
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&)261359
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&)211554
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&)387831
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&)2469692
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&)2942935
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&)20010
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&)352554
double const mrs_lib::get_mutexed<double const>(std::mutex&, double const&)764416
mrs_uav_managers::Controller::ControlOutput mrs_lib::get_mutexed<mrs_uav_managers::Controller::ControlOutput>(std::mutex&, mrs_uav_managers::Controller::ControlOutput&)841949
mrs_uav_trackers::mpc_trackerConfig mrs_lib::get_mutexed<mrs_uav_trackers::mpc_trackerConfig>(std::mutex&, mrs_uav_trackers::mpc_trackerConfig&)84480
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&)88415
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&)32942
mrs_uav_controllers::mpc_controllerConfig mrs_lib::get_mutexed<mrs_uav_controllers::mpc_controllerConfig>(std::mutex&, mrs_uav_controllers::mpc_controllerConfig&)161101
mrs_uav_controllers::se3_controllerConfig mrs_lib::get_mutexed<mrs_uav_controllers::se3_controllerConfig>(std::mutex&, mrs_uav_controllers::se3_controllerConfig&)58289
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&)952846
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>&)312035
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>&)198426
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>&)272168
mrs_msgs::ObstacleSectors_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ObstacleSectors_<std::allocator<void> > >(std::mutex&, mrs_msgs::ObstacleSectors_<std::allocator<void> >&)435
mrs_msgs::ReferenceStamped_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(std::mutex&, mrs_msgs::ReferenceStamped_<std::allocator<void> >&)1113
mrs_msgs::VelocityReference_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::VelocityReference_<std::allocator<void> > >(std::mutex&, mrs_msgs::VelocityReference_<std::allocator<void> >&)818
mrs_msgs::DynamicsConstraints_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraints_<std::allocator<void> >&)292162
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> >&)81217
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> >&)168673
mrs_msgs::UavState_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::UavState_<std::allocator<void> > >(std::mutex&, mrs_msgs::UavState_<std::allocator<void> >&)1074136
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&)799966
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&)427523
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> >&)91325
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> >&)252073
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> > >&)735867
std::optional<double> mrs_lib::get_mutexed<std::optional<double> >(std::mutex&, std::optional<double>&)19436
double mrs_lib::get_mutexed<double>(std::mutex&, double&)1811186
int mrs_lib::get_mutexed<int>(std::mutex&, int&)9441
unsigned int mrs_lib::get_mutexed<unsigned int>(std::mutex&, unsigned int&)1047136
auto mrs_lib::set_mutexed<mrs_uav_managers::Controller::ControlOutput>(std::mutex&, mrs_uav_managers::Controller::ControlOutput, mrs_uav_managers::Controller::ControlOutput&)111575
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&)16599
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&)4684
auto mrs_lib::set_mutexed<mrs_uav_controllers::mpc_controllerConfig>(std::mutex&, mrs_uav_controllers::mpc_controllerConfig, mrs_uav_controllers::mpc_controllerConfig&)218
auto mrs_lib::set_mutexed<mrs_uav_controllers::se3_controllerConfig>(std::mutex&, mrs_uav_controllers::se3_controllerConfig, mrs_uav_controllers::se3_controllerConfig&)219
auto mrs_lib::set_mutexed<mrs_uav_trajectory_generation::drsConfig>(std::mutex&, mrs_uav_trajectory_generation::drsConfig, mrs_uav_trajectory_generation::drsConfig&)109
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> >&)129683
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> >&)2498
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> >&)552749
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> >&)166
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> >&)1103063
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> >&)423085
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&)799064
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&)427593
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> >&)38271
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> >&)231587
auto mrs_lib::set_mutexed<double>(std::mutex&, double, double&)1248651
+
+
+ + + +
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..827b04e129 --- /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-12-17 22:35:29Functions: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     1044319 : std::tuple<Args...> get_mutexed(std::mutex& mut, Args&... args) {
+      49             : 
+      50     2088638 :   std::scoped_lock lock(mut);
+      51             : 
+      52     1044319 :   std::tuple result = std::tuple(args...);
+      53             : 
+      54     2088638 :   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    21035299 : T get_mutexed(std::mutex& mut, T& arg) {
+      69             : 
+      70    34136005 :   std::scoped_lock lock(mut);
+      71             : 
+      72    42068753 :   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     5089814 : auto set_mutexed(std::mutex& mut, const T what, T& where) {
+     117             : 
+     118     8902545 :   std::scoped_lock lock(mut);
+     119             : 
+     120     5082277 :   where = what;
+     121             : 
+     122    10174000 :   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..8daaccabb2 --- /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-12-17 22:35:29Functions: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&)42
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)88
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)90
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&)109
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)109
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)109
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)201
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)436
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)436
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&)545
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)752
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&)756
bool mrs_lib::ParamLoader::loadParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&, double const&)872
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&)1199
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)1199
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> >&)1199
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, std::basic_string_view<char, std::char_traits<char> >)1841
mrs_lib::ParamLoader::setPrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2124
mrs_lib::ParamLoader::addYamlFileFromParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2924
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&)3807
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)3808
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> > > >&)3808
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, bool, std::basic_string_view<char, std::char_traits<char> >)4226
mrs_lib::ParamLoader::loadedSuccessfully()4713
void mrs_lib::ParamLoader::printValue<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int const&)5500
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)5500
bool mrs_lib::ParamLoader::loadParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&)5500
bool mrs_lib::ParamLoader::loadParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&, bool const&)6971
mrs_lib::ParamLoader::addYamlFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)12174
bool mrs_lib::ParamLoader::loadParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&)12851
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> >&)18046
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&)18634
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)18634
void mrs_lib::ParamLoader::printValue<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool const&)19822
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)19822
bool mrs_lib::ParamLoader::loadParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&)43730
void mrs_lib::ParamLoader::printValue<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double const&)44602
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)44602
mrs_lib::ParamLoader::check_duplicit_loading(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)94336
+
+
+ + + +
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..e7bdcfb5ff --- /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-12-17 22:35:29Functions: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&)545
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&)3807
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&)18634
void mrs_lib::ParamLoader::printValue<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool const&)19822
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&)756
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&)1199
void mrs_lib::ParamLoader::printValue<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double const&)44602
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&)5500
mrs_lib::ParamLoader::addYamlFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)12174
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)752
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)436
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&)109
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)90
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)109
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)88
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()4713
mrs_lib::ParamLoader::addYamlFileFromParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2924
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&)94336
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)436
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)109
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)201
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)18634
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)3808
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)1199
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)19822
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)44602
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)5500
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> >&)18046
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&)42
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> > > >&)3808
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> >&)1199
bool mrs_lib::ParamLoader::loadParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&)12851
bool mrs_lib::ParamLoader::loadParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&, bool const&)6971
bool mrs_lib::ParamLoader::loadParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&)43730
bool mrs_lib::ParamLoader::loadParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&, double const&)872
bool mrs_lib::ParamLoader::loadParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&)5500
mrs_lib::ParamLoader::setPrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2124
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, std::basic_string_view<char, std::char_traits<char> >)1841
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, bool, std::basic_string_view<char, std::char_traits<char> >)4226
+
+
+ + + +
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..f0c870769f --- /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-12-17 22:35:29Functions: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             :   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       88558 :   void printValue(const std::string& name, const T& value)
+      96             :   {
+      97       88558 :     if (m_node_name.empty())
+      98       49309 :       std::cout << "\t" << name << ":\t" << value << std::endl;
+      99             :     else
+     100       39249 :       ROS_INFO_STREAM("[" << m_node_name << "]: parameter '" << name << "':\t" << value);
+     101       88558 :   }
+     102             : 
+     103             :   template <typename T>
+     104        5006 :   void printValue(const std::string& name, const std::vector<T>& value)
+     105             :   {
+     106       10012 :     std::stringstream strstr;
+     107        5006 :     if (m_node_name.empty())
+     108        2431 :       strstr << "\t";
+     109        5006 :     strstr << name << ":\t";
+     110        5006 :     size_t it = 0;
+     111       17499 :     for (const auto& elem : value)
+     112             :     {
+     113       12493 :       strstr << elem;
+     114       12493 :       if (it < value.size() - 1)
+     115        8047 :         strstr << ", ";
+     116       12493 :       it++;
+     117             :     }
+     118        5006 :     if (m_node_name.empty())
+     119        2431 :       std::cout << strstr.str() << std::endl;
+     120             :     else
+     121        2575 :       ROS_INFO_STREAM("[" << m_node_name << "]: parameter '" << strstr.str());
+     122        5006 :   }
+     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         764 :   void printValue(const std::string& name, const MatrixX<T>& value)
+     147             :   {
+     148        1528 :     std::stringstream strstr;
+     149             :     /* const Eigen::IOFormat fmt(4, 0, ", ", "\n", "\t\t[", "]"); */
+     150             :     /* strstr << value.format(fmt); */
+     151        2292 :     const Eigen::IOFormat fmt;
+     152         764 :     strstr << value.format(fmt);
+     153         764 :     if (m_node_name.empty())
+     154         110 :       std::cout << "\t" << name << ":\t" << std::endl << strstr.str() << std::endl;
+     155             :     else
+     156         654 :       ROS_INFO_STREAM("[" << m_node_name << "]: parameter '" << name << "':" << std::endl << strstr.str());
+     157         764 :   }
+     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       94336 :   bool check_duplicit_loading(const std::string& name)
+     216             :   {
+     217       94336 :     if (m_loaded_params.count(name))
+     218             :     {
+     219           3 :       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       94333 :       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         762 :   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        1524 :     const std::string name_prefixed = m_prefix + name;
+     235        1524 :     MatrixX<T> loaded = default_value;
+     236         762 :     bool used_rosparam_value = false;
+     237             :     // first, check if the user already tried to load this parameter
+     238         762 :     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         762 :     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         762 :     const bool expect_zero_matrix = rows == 0;
+     250         762 :     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         762 :     bool cur_load_successful = true;
+     261         762 :     bool check_size_exact = true;
+     262         762 :     if (cols <= 0)  // this means that the cols dimension is dynamic or a zero matrix is expected
+     263         199 :       check_size_exact = false;
+     264             : 
+     265        1524 :     std::vector<T> tmp_vec;
+     266             :     // try to load the parameter
+     267         762 :     const bool success = m_pp.getParam(name_prefixed, tmp_vec);
+     268             :     // check if the loaded vector has correct length
+     269         762 :     bool correct_size = (int)tmp_vec.size() == rows * cols;
+     270         762 :     if (!check_size_exact && !expect_zero_matrix)
+     271         198 :       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         762 :     if (success && correct_size)
+     274             :     {
+     275             :       // if successfully loaded, everything is in order
+     276             :       // transform the vector to the matrix
+     277         644 :       if (cols <= 0 && rows > 0)
+     278         198 :         cols = tmp_vec.size() / rows;
+     279         644 :       if (swap)
+     280          88 :         std::swap(rows, cols);
+     281         644 :       loaded = Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>, Eigen::Unaligned>(tmp_vec.data(), rows, cols);
+     282         644 :       used_rosparam_value = true;
+     283             :     } else
+     284             :     {
+     285         118 :       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         118 :       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         762 :     if (cur_load_successful)
+     309             :     {
+     310         759 :       if (m_print_values && printValues)
+     311         754 :         printValue(name_prefixed, loaded);
+     312         759 :       m_loaded_params.insert(name_prefixed);
+     313             :     } else
+     314             :     {
+     315           3 :       m_load_successful = false;
+     316             :     }
+     317             :     // finally, return the resulting value
+     318         762 :     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         116 :   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         348 :     const auto [dynamic, loaded_ok] = loadMatrixX(name, MatrixX<T>(default_value), rows, cols, optional, unique, NO_SWAP);
+     327         232 :     return {dynamic, loaded_ok};
+     328             :   }
+     329             :   //}
+     330             : 
+     331             :   /* loadMatrixKnown_internal helper function for loading EigenXd matrices with known dimensions //{ */
+     332             :   template <typename T>
+     333         440 :   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         880 :     MatrixX<T> loaded = default_value;
+     336             :     // first, check that at least one dimension is set
+     337         440 :     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         440 :     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         201 :   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         402 :     MatrixX<T> loaded = default_value;
+     353             : 
+     354             :     // next, check that at least one dimension is set
+     355         201 :     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         201 :     swap_t swap = NO_SWAP;
+     363         201 :     if (rows <= 0)
+     364             :     {
+     365          88 :       std::swap(rows, cols);
+     366          88 :       swap = SWAP;
+     367             :     }
+     368         201 :     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       93571 :   std::pair<T, bool> load(const std::string& name, const T& default_value, optional_t optional = OPTIONAL, unique_t unique = UNIQUE)
+     453             :   {
+     454      187141 :     const std::string name_prefixed = m_prefix + name;
+     455      117218 :     T loaded = default_value;
+     456       93571 :     if (unique && check_duplicit_loading(name_prefixed))
+     457           2 :       return {loaded, false};
+     458             : 
+     459       93569 :     bool cur_load_successful = true;
+     460             :     // try to load the parameter
+     461       93569 :     const bool success = m_pp.getParam(name_prefixed, loaded);
+     462       93569 :     if (!success)
+     463             :     {
+     464             :       // if it was not loaded, set the default value
+     465        2188 :       loaded = default_value;
+     466        2188 :       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       93569 :     if (cur_load_successful)
+     475             :     {
+     476             :       // everything is fine and just print the name_prefixed and value if required
+     477       93567 :       if (m_print_values)
+     478       93567 :         printValue(name_prefixed, loaded);
+     479             :       // mark the param name_prefixed as successfully loaded
+     480       93567 :       m_loaded_params.insert(name_prefixed);
+     481             :     } else
+     482             :     {
+     483           2 :       m_load_successful = false;
+     484             :     }
+     485             :     // finally, return the resulting value
+     486       93569 :     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        4226 :   ParamLoader(const ros::NodeHandle& nh, bool printValues = true, std::string_view node_name = std::string())
+     499        4226 :       : m_load_successful(true),
+     500             :         m_print_values(printValues),
+     501             :         m_node_name(node_name),
+     502             :         m_nh(nh),
+     503        4226 :         m_pp(nh, m_node_name)
+     504             :   {
+     505             :     /* std::cout << "Initialized1 ParamLoader for node " << node_name << std::endl; */
+     506        4226 :   }
+     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        1841 :   ParamLoader(const ros::NodeHandle& nh, std::string_view node_name)
+     516        1841 :       : ParamLoader(nh, true, node_name)
+     517             :   {
+     518             :     /* std::cout << "Initialized2 ParamLoader for node " << node_name << std::endl; */
+     519        1841 :   }
+     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        2124 :   void setPrefix(const std::string& prefix)
+     542             :   {
+     543        2124 :     m_prefix = prefix;
+     544        2124 :   }
+     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       12174 :   bool addYamlFile(const std::string& filepath)
+     571             :   {
+     572       12174 :     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        2924 :   bool addYamlFileFromParam(const std::string& param_name)
+     585             :   {
+     586        5848 :     std::string filepath;
+     587        2924 :     if (!loadParam(param_name, filepath))
+     588           0 :       return false;
+     589        2924 :     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        4713 :   bool loadedSuccessfully()
+     600             :   {
+     601        4713 :     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        7887 :   bool loadParam(const std::string& name, T& out_value, const T& default_value)
+     640             :   {
+     641        7887 :     const auto [ret, success] = load<T>(name, default_value, OPTIONAL, UNIQUE);
+     642        7887 :     out_value = ret;
+     643        7931 :     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         546 :   T loadParam2(const std::string& name, const T& default_value)
+     658             :   {
+     659        1092 :     const auto loaded = load<T>(name, default_value, OPTIONAL, UNIQUE);
+     660        1092 :     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       85137 :   bool loadParam(const std::string& name, T& out_value)
+     714             :   {
+     715      108193 :     const auto [ret, success] = load<T>(name, T(), COMPULSORY, UNIQUE);
+     716       85137 :     out_value = ret;
+     717      108193 :     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         112 :   bool loadMatrixStatic(const std::string& name, Eigen::Matrix<T, rows, cols>& mat, const Eigen::MatrixBase<Derived>& default_value)
+    1001             :   {
+    1002         112 :     const auto [ret, loaded_ok] = loadMatrixStatic_internal<rows, cols, T>(name, Eigen::Matrix<T, rows, cols>(default_value), OPTIONAL, UNIQUE);
+    1003         112 :     mat = ret;
+    1004         112 :     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         438 :   bool loadMatrixKnown(const std::string& name, MatrixX<T>& mat, int rows, int cols)
+    1076             :   {
+    1077         876 :     const auto [ret, loaded_ok] = loadMatrixKnown_internal(name, MatrixX<T>(), rows, cols, COMPULSORY, UNIQUE);
+    1078         438 :     mat = ret;
+    1079         876 :     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          90 :   bool loadMatrixDynamic(const std::string& name, MatrixX<T>& mat, int rows, int cols)
+    1171             :   {
+    1172         180 :     const auto [ret, loaded_ok] = loadMatrixDynamic_internal(name, MatrixX<T>(), rows, cols, COMPULSORY, UNIQUE);
+    1173          90 :     mat = ret;
+    1174         180 :     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         111 :   bool loadMatrixDynamic(const std::string& name, MatrixX<T>& mat, const Eigen::MatrixBase<Derived>& default_value, int rows, int cols)
+    1195             :   {
+    1196         222 :     const auto [ret, loaded_ok] = loadMatrixDynamic_internal(name, MatrixX<T>(default_value), rows, cols, OPTIONAL, UNIQUE);
+    1197         111 :     mat = ret;
+    1198         222 :     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          88 :   MatrixX<T> loadMatrixDynamic2(const std::string& name, int rows, int cols)
+    1217             :   {
+    1218          88 :     MatrixX<T> ret;
+    1219          88 :     loadMatrixDynamic(name, ret, rows, cols);
+    1220          88 :     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..ca6f5035d249472f118565a7a5ce911c7aa5bc2c GIT binary patch literal 4405 zcmV-55z6j~P)M!cuaMW5tOVe@8LxxB=96S(OnjTOS z;izrFQQvfzZ$YRS{^$bmjTs$|vI&Rc2=Nbd)OON`qH&y202K*DJ!%h{@Q+z!j%w|Y zZ+sKntLqXPJ@_5t9@xbNSwoKRy4{VUdX8w(%cA;q4 zu{Db?T_SWr^xK1wJW3(HfKdk?4S%v|THi-kiVk>-xmmg6FN3F%xI*2M$`P4||2?s0 z_DSFZ766R`ZE=M9!Kk%_9L!KWT8`>#J6CN=l?k_WG9;O*2vw8@iL0D~aWJ~k1SO?y zFq)292BVvp7*%+UXID3^y-;_+3(dzJKD6pZ~u9`(9uFe9>hX%v}s-Mlsb0&lOLH`#$lkVgV`r#MfW@#G|9MB0S;M z4Nt$6w_!pa9(&rb+Q8(8&fga$pa$Mm30>hdugHaMbE^yYj zB{gjjxM0}+lyzp}Tqv3*xsC~!(LAn%3(92aF#SiU)WVy7FspoKFnH~*Mo=V{bC+dm zTG36*VC>2zer{ViQn^x}&Q&gm1)326hieq8P{QX$1Z)e~kzw~!GOZl4bgF1L(P6Qm z{fS8EH5n3>C}N=7CbVT|SmRhrpRN_ZQg$9jU1gS|rgh!ar{b0=40&Cn-WPq`DmMBHwr^PtVjD}~L!uyiK zu6bEVrb~@b7o|j&qz*&E1)H2HuDZw2>Ru2^1tSEd$=%b(?wklhtsJ*A(}0<6X6AnQ zD5h{64C~sqPhmMTQoUC(@La2!e&Q4=Y^ik$$GOi5gHeY7(n$52)|zJZ)e05FJ!8|- zoGe>okrA5Ra90@~FEzn?D#x9YAsrnT=8j$He4*(^$6grHI-wemhmShT0I=ljWbWxw z`m*r}jPw2CQ3%Ig`N;Y>bI3$$v`E69o1sW*s1Iu&RZ24%tQj;t7`$FQhP3b$m_c%f zOU#}Nz&nc z-Z=uh7ZQB3H6-v~NeoE}-^rVOOTwS^SItoyhcJT>bCxEl!4VKXSAvTr=ZF=aXaK?T-76evJ%gFerBE}SSvu;&a-6e? zTq_LoB4lrp<1Ab45ZcThI!;_%t(M$HXa`%wYM{TN#6fnQK1PwxBj_T>;fQdt2~p4` z+yZCb{>-a|edRhMh|4Cs(YD@@!W@E2!)=9*cKcxr&vJ~a(k&|zDXSjIwPBK#h>w1S{oQZvcjM2ayWnC^L2UCja7oleaN-KI45mGVgJJ?)V3X2fQ2|LM@rH>~p`H*6bnmR$qD56~iu`Ps|*y1Kcfgj|MIy_Y~l2 zdB{uW7oGR1pLbJsH4O7RD;RBT6moI^Tr)PR|}tBKZZ zShXwu3hN3Z$F5vgxM3(Y$j40uU$vt-*mV`FQbi6)#`I2>f|ei`vb=CBsnbL?X?he; zY-dg7Fi4v-LJx#YKzpe3G7fE7x@WB~JJt2Kwa_v%3krGwvvMD4nzYK=hb1h8gcbSq z2oX85c%LoEbKML!3ggR6eX}bv-p`jdU?*lTEnXcpr>2UwS6kJNdaw5t}$S3uepQ?0}U2n+N`{tx# zrod6WC6hXzlvFIjK#U*q8}D?vWi{mCf|znJ+U5O2R>U`>J}?fSwyRQ5w=O}Et-JQ+ z_*uR>;5vR)YLd1BO|Cfk1`80`@>P$wZTV_Wvw;>sOiMx7*JvK~yRLKflU z?L=vx3fzshr>_KrM_sWgHXr!v3w2B}vBm8~|5o4@c_CW+7<1x-qR~j5@N@y~LN50w zm$ES;9Jz{}GdMPY)|m)D6}a*i3w+`G@>k`|yaXuz;#_CP0kBF?g+?sic|qA5nDHor z{OHi?hoAgl182>La5Tb9B!+z4Vm=90;CkF2itABX8!I=xrO0<=Hc~I^vh+Nxro6;6 zXBR+^*q^k44H8;7iW7jd6G0)uf=((KN)z92S=+@)7~g|9s#`v75&1a$*aLPd=6N5; zBS`_9@km>np)?0L$_df?i4tLP-s!Z*NM#j*M@-6HR)(5^__GVX>{TH+c&rh#?Q!U? zII2J>0V{jR<%6gYVI>2ryc3~kDz>Qexw<}Wyx4;t5fg6;U*?@xg9?Bs0!r*wH^tJb z)L_l&)qp#I?T&z*At3@DG%J(Y7xGF4l`Ju6i!Dw2VAg&5YM+t1W~nLPC)M+F=DDU* zQv;cuBFfll01ld&G%MYWRTntIQ{g%G&5WZkGII?(9sB6q8T-uiX8IB{!wb1+fHsP= z&tJ2vbnLtx=#J?``^N3|DPIJWgMc5i(t*PMk^bMau& zg-Lu6qmzVyX8^j(MF51*&uqEdap-G`QuZ8rC$^2w`Kz6eoXmILNU_L9=EU)aCQre! z{-9tkSK+Y3l0gu6q(@aODx1?v1Z2iWO8TKWiR3{y31*10)*=X zEfh201*WAV*--9zfwO{-DG@e}z_d3?>6My($5P;UMQa~RF_7aYRQQAn)lk8YG7nSJkLY#nODpAf z>CAcYE<4eG&*28hviV)Ud1HN!EjAh9_4N7^0laSn5QLIFT7Gz@V;>s;^GOYR3;><+ zS|>c#0GwEUaDnyT;ahxl;&Q>p_Jng|3jdxl#ql+O$dM#d4Lz!a^>~J2icwYJ=!#~Z z0u)Qw_^lN4ktqOv(bL~Z0ZtATPFL5(Xmt0J>6~)Zgo2g6%KyB5MNd$ zJJ#!T^KiZXr1lz)PuJ_qyWvy3>nLNlYhf+@=IfSe#yzjG;eRHC(^qc~pUKU?_B*L< z^?3D9BL4BN4`#zBI?bNSp5Qdw>MHL4inDZi-#B!hMvjO0YxddGLzcoJ zyxC$Tz5cuY`k}NLdQ3KaODIJOET_w)&F#GTxbPa=`%B^V>#FPfIQA)VZ0dH|bW^w6 zgoSQTmr0x3dGklFN7sAm_2u30Dc<$zX>%iQJ~)@NQt&e(R9s#sZW~7iEbZM2=y6=arj6s)X>PsdGBt`Vw(5ht0@!p%P7F;dMR)x4z7w$r_z(s% z8hhJd(6DewMlmMJ1@H)jlEew^$ix#S5u9SsCtRGik52-wEjG)5Tb={(-oT}##8)8f zDPRw@fGF}F>;iGL4MquEWH0gvj&jI-Xq!GKwa>DI0-1L76v;=y8Gm`H?$I38>%Pa2<U^YQn@}I;(=D8BieSRJpQf vPka60xxVrg8n%6SD(M_$3fAV^)63WYf}?V554M|r00000NkvXXu0mjfEZut{ 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..071d74fefd --- /dev/null +++ b/mrs_lib/include/mrs_lib/publisher_handler.h.func-sort-c.html @@ -0,0 +1,568 @@ + + + + + + + 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-12-17 22:35:29Functions:12012298.4 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::PublisherHandler_impl<geometry_msgs::PointStamped_<std::allocator<void> > >::~PublisherHandler_impl()0
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()32
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::~PublisherHandler_impl()32
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::~PublisherHandler()64
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<std_msgs::String_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::~PublisherHandler_impl()110
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~PublisherHandler_impl()116
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::~PublisherHandler_impl()218
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::PublisherHandler()219
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::PublisherHandler()226
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::PublisherHandler()226
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::~PublisherHandler_impl()226
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::~PublisherHandler_impl()312
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::~PublisherHandler()327
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::~PublisherHandler()327
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::~PublisherHandler()327
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::~PublisherHandler()327
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::~PublisherHandler()327
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::~PublisherHandler()327
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::~PublisherHandler()327
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::~PublisherHandler()327
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::~PublisherHandler()329
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~PublisherHandler()342
mrs_lib::PublisherHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::PublisherHandler()402
mrs_lib::PublisherHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::~PublisherHandler()402
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::PublisherHandler()428
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~PublisherHandler_impl()428
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::PublisherHandler()436
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::~PublisherHandler()436
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::PublisherHandler()436
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::~PublisherHandler_impl()436
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::~PublisherHandler_impl()436
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::~PublisherHandler()452
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::~PublisherHandler_impl()528
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler()625
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::~PublisherHandler()625
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~PublisherHandler_impl()625
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::PublisherHandler()654
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::~PublisherHandler_impl()654
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler()671
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::PublisherHandler()742
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::PublisherHandler()804
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::~PublisherHandler_impl()804
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~PublisherHandler()856
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::~PublisherHandler()872
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::~PublisherHandler()872
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::~PublisherHandler()1054
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::PublisherHandler()1136
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::~PublisherHandler()1308
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::~PublisherHandler()1608
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~PublisherHandler()1761
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::~PublisherHandler()133688
+
+
+ + + +
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..808f92e28e --- /dev/null +++ b/mrs_lib/include/mrs_lib/publisher_handler.h.func.html @@ -0,0 +1,568 @@ + + + + + + + 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-12-17 22:35:29Functions:12012298.4 %
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()436
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::~PublisherHandler()872
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::PublisherHandler()218
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::~PublisherHandler()436
mrs_lib::PublisherHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::PublisherHandler()402
mrs_lib::PublisherHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::~PublisherHandler()402
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::PublisherHandler()226
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~PublisherHandler()342
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::PublisherHandler()654
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::~PublisherHandler()1308
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::PublisherHandler()1136
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~PublisherHandler()1761
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::PublisherHandler()428
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~PublisherHandler()856
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()109
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::~PublisherHandler()327
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::~PublisherHandler()327
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::~PublisherHandler()327
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::PublisherHandler()804
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::~PublisherHandler()1608
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::PublisherHandler()742
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::~PublisherHandler()1054
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::PublisherHandler()219
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::~PublisherHandler()329
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler()625
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::~PublisherHandler()625
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::~PublisherHandler()327
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::~PublisherHandler()327
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::~PublisherHandler()327
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::~PublisherHandler()327
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::~PublisherHandler()327
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::PublisherHandler()226
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::~PublisherHandler()452
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler()671
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::~PublisherHandler()133688
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::PublisherHandler()32
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::~PublisherHandler()64
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<std_msgs::Int64_<std::allocator<void> > >::~PublisherHandler()2
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::PublisherHandler()109
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::~PublisherHandler()218
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::PublisherHandler()436
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::~PublisherHandler()872
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::~PublisherHandler_impl()436
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::~PublisherHandler_impl()218
mrs_lib::PublisherHandler_impl<geometry_msgs::PointStamped_<std::allocator<void> > >::~PublisherHandler_impl()0
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~PublisherHandler_impl()116
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::~PublisherHandler_impl()654
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~PublisherHandler_impl()625
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~PublisherHandler_impl()428
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()109
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::~PublisherHandler_impl()804
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::~PublisherHandler_impl()312
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::~PublisherHandler_impl()110
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()109
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::~PublisherHandler_impl()109
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::~PublisherHandler_impl()226
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::~PublisherHandler_impl()528
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::~PublisherHandler_impl()32
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::~PublisherHandler_impl()109
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()109
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::~PublisherHandler_impl()436
+
+
+ + + +
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..26c22ff65d --- /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-12-17 22:35:29Functions:12012298.4 %
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        7437 :   ~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       10637 :   PublisherHandler(void){};
+     102             : 
+     103             :   /**
+     104             :    * @brief generic destructor
+     105             :    */
+     106      150563 :   ~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         242 :   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..d16176d6ec --- /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-12-17 22:35:29Functions: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)93838
mrs_lib::quadratic_throttle_model::throttleToForce(mrs_lib::quadratic_throttle_model::MotorParams_t, double)131243
+
+
+ + + +
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..f3e17a3c94 --- /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-12-17 22:35:29Functions: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)93838
mrs_lib::quadratic_throttle_model::throttleToForce(mrs_lib::quadratic_throttle_model::MotorParams_t, double)131243
+
+
+ + + +
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..ffb5e49a75 --- /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-12-17 22:35:29Functions: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      131243 : double inline throttleToForce(const MotorParams_t motor_params, const double throttle) {
+      20             : 
+      21      131243 :   return motor_params.n_motors * pow((throttle - motor_params.B) / motor_params.A, 2);
+      22             : }
+      23             : 
+      24       93838 : double inline forceToThrottle(const MotorParams_t motor_params, const double force) {
+      25             : 
+      26       93838 :   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..d113061a6b --- /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-12-17 22:35:29Functions: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<!(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<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&)104
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&)199
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
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&)296
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&)296
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&)592
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&)592
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&)635
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&)2057
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&)6890
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&)16924
+
+
+ + + +
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..e5c1494800 --- /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-12-17 22:35:29Functions: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&)16924
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&)6890
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&)635
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&)2057
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&)296
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&)592
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&)104
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&)296
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&)199
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&)592
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..dbd5eca9ae --- /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-12-17 22:35:29Functions: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       16822 :       do
+      81             :       {
+      82       16924 :         cur_sc.stamp = hist_it->stamp;
+      83             :         // do the correction if current history point is a measurement
+      84       16924 :         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       16924 :         ros::Time next_stamp = to_stamp;
+      90       16924 :         if ((hist_it + 1) != std::end(m_history) && (hist_it + 1)->stamp <= to_stamp)
+      91       16822 :           next_stamp = (hist_it + 1)->stamp;
+      92             : 
+      93             :         // do the prediction
+      94       16924 :         cur_sc = predictFrom(cur_sc, *hist_it, cur_stamp, next_stamp);
+      95       16924 :         cur_stamp = next_stamp;
+      96             : 
+      97             :         // increment the history pointer
+      98       16924 :         hist_it++;
+      99       16924 :       } 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         592 :     std::enable_if_t<check, statecov_t> predictTo(const ros::Time& to_stamp)
+     118             :     {
+     119         592 :       assert(!m_history.empty());
+     120         592 :       const auto& info = m_history.front();
+     121         592 :       auto sc = predictFrom(m_sc, info, info.stamp, to_stamp);
+     122         592 :       sc.stamp = to_stamp;
+     123         592 :       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        6990 :         for (auto it = added + 1; it != std::end(m_history) && it->is_measurement; it++)
+     151        6790 :           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         199 :     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         199 :       if (m_history.empty())
+     170           2 :         m_history.push_back({stamp});
+     171         199 :       m_history.front().u = u;
+     172         199 :       m_history.front().Q = Q;
+     173         199 :       m_history.front().stamp = stamp;
+     174         199 :       m_history.front().predict_model = model;
+     175         199 :     }
+     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         104 :     std::enable_if_t<check> addInputChange(const u_t& u, [[maybe_unused]] const ros::Time& stamp, const ModelPtr& model = nullptr)
+     221             :     {
+     222         104 :       if (m_history.empty())
+     223           0 :         m_history.push_back({stamp});
+     224         104 :       m_history.front().u = u;
+     225         104 :       m_history.front().stamp = stamp;
+     226         104 :       m_history.front().predict_model = model;
+     227         104 :     }
+     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         296 :     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         296 :       if (m_history.empty())
+     325           0 :         m_history.push_back({stamp});
+     326         296 :       auto& info = m_history.front();
+     327         296 :       const ros::Time to_stamp = stamp > info.stamp ? stamp : info.stamp;
+     328         296 :       const auto sc = predictTo(to_stamp);
+     329         296 :       info.z = z;
+     330         296 :       info.R = R;
+     331         296 :       info.stamp = to_stamp;
+     332         296 :       info.is_measurement = true;
+     333         296 :       info.meas_id = meas_id;
+     334         296 :       info.correct_model = model;
+     335         296 :       m_sc = correctFrom(sc, info);
+     336         296 :     }
+     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         637 :       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        6890 :       void updateUsing(const info_t& info)
+     431             :       {
+     432        6890 :         u = info.u;
+     433        6890 :         Q = info.Q;
+     434        6890 :         predict_model = info.predict_model;
+     435        6890 :       };
+     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        2057 :     static bool earlier(const info_t& ob1, const info_t& ob2)
+     526             :     {
+     527        2057 :       return ob1.stamp < ob2.stamp;
+     528             :     }
+     529             :     //}
+     530             : 
+     531             :     /* predictFrom() method //{ */
+     532       17516 :     statecov_t predictFrom(const statecov_t& sc, const info_t& inpt, const ros::Time& from_stamp, const ros::Time& to_stamp)
+     533             :     {
+     534       35032 :       const auto model = inpt.predict_model == nullptr ? m_default_model : inpt.predict_model;
+     535       17516 :       const auto dt = (to_stamp - from_stamp).toSec();
+     536       35032 :       return model->predict(sc, inpt.u, inpt.Q, dt);
+     537             :     }
+     538             :     //}
+     539             : 
+     540             :     /* correctFrom() method //{ */
+     541        5446 :     statecov_t correctFrom(const statecov_t& sc, const info_t& meas)
+     542             :     {
+     543        5446 :       assert(meas.is_measurement);
+     544       10892 :       const auto model = meas.correct_model == nullptr ? m_default_model : meas.correct_model;
+     545        5446 :       auto sc_tmp = sc;
+     546       10892 :       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..9c89a0ed97 --- /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-12-17 22:35:29Functions: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..1a9cece99d --- /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-12-17 22:35:29Functions: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..3354cd52c7 --- /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-12-17 22:35:29Functions: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..b2de66c52c --- /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-12-17 22:35:29Functions: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..64dd066606 --- /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-12-17 22:35:29Functions: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..3fd0376397 --- /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-12-17 22:35:29Functions: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..a517cf8c65 --- /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-12-17 22:35:29Functions: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..c73a637dc9 --- /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-12-17 22:35:29Functions: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..803b3994a5 --- /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-12-17 22:35:29Functions: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..a93b7102d9 --- /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-12-17 22:35:29Functions: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..c748ee089a --- /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-12-17 22:35:29Functions: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..69a6509149 --- /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-12-17 22:35:29Functions: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..f96dec3c9e --- /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-12-17 22:35:29Functions: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&)5819196
+
+
+ + + +
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..12d27e67ab --- /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-12-17 22:35:29Functions: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&)5819196
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..1c146938bc --- /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-12-17 22:35:29Functions: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     5819196 :     time_point(const std::string& label) : ros_time(ros::Time::now()), chrono_time(std::chrono::steady_clock::now()), label(label) {
+      93     5819000 :     }
+      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-12-17 22:35:29Functions: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()109
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::ServiceClientHandler()109
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::~ServiceClientHandler_impl()109
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::~ServiceClientHandler_impl()109
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler()111
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::~ServiceClientHandler_impl()111
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::~ServiceClientHandler()218
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::~ServiceClientHandler()218
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::~ServiceClientHandler()222
mrs_lib::ServiceClientHandler<mrs_msgs::String>::ServiceClientHandler()327
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::~ServiceClientHandler_impl()327
mrs_lib::ServiceClientHandler<mrs_msgs::String>::~ServiceClientHandler()654
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::ServiceClientHandler()656
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::~ServiceClientHandler_impl()656
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::ServiceClientHandler()1192
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::~ServiceClientHandler_impl()1192
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::~ServiceClientHandler()1312
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::~ServiceClientHandler()2275
+
+
+ + + +
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..ff5d3f0d15 --- /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-12-17 22:35:29Functions: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()111
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::~ServiceClientHandler()222
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler()109
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::~ServiceClientHandler()218
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::ServiceClientHandler()109
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::~ServiceClientHandler()218
mrs_lib::ServiceClientHandler<mrs_msgs::String>::ServiceClientHandler()327
mrs_lib::ServiceClientHandler<mrs_msgs::String>::~ServiceClientHandler()654
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::ServiceClientHandler()656
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::~ServiceClientHandler()1312
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::ServiceClientHandler()1192
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::~ServiceClientHandler()2275
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()111
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::~ServiceClientHandler_impl()109
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::~ServiceClientHandler_impl()109
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::~ServiceClientHandler_impl()327
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::~ServiceClientHandler_impl()656
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::~ServiceClientHandler_impl()1192
+
+
+ + + +
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..f69a9a212b --- /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-12-17 22:35:29Functions: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        2507 :   ~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        2507 :   ServiceClientHandler(void){};
+     135             : 
+     136             :   /**
+     137             :    * @brief generic destructor
+     138             :    */
+     139        4905 :   ~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..ee3a661aa9 --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.func-sort-c.html @@ -0,0 +1,3528 @@ + + + + + + + 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:445186.3 %
Date:2024-12-17 22:35:29Functions:39886246.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> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration 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::Correction<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<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::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<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::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)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<void (mrs_uav_state_estimators::ProcMagDeclination<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcMagDeclination<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::ProcMagDeclination<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcMagDeclination<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::ProcMagDeclination<2>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcMagDeclination<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::ProcMagDeclination<2>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcMagDeclination<2>*)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<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::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<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::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)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::NavSatFix_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::ProcMagDeclination<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::ProcMagDeclination<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcMagDeclination<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::ProcMagDeclination<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::ProcMagDeclination<2>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcMagDeclination<2>*)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::start()0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::MagneticField_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::MagneticField_<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::MagneticField_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::MagneticField_<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::MagneticField_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<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::MagneticField_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<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::MagneticField_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >&&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::Float64Stamped_<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::Float64Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::Float64Stamped_<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::Float64Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<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::Float64Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<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::Float64Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration 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> > >::getNumPublishers() 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::MagneticField_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::getNumPublishers() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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<nav_msgs::Odometry_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::getNumPublishers() 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::Correction<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<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::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<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::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() 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::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::ProcMagDeclination<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcMagDeclination<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::ProcMagDeclination<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcMagDeclination<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::ProcMagDeclination<2>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcMagDeclination<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::ProcMagDeclination<2>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcMagDeclination<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::MagneticField_<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::MagneticField_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::MagneticField_<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::MagneticField_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{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<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::Float64Stamped_<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::Float64Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::Float64Stamped_<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::Float64Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{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> > >::SubscribeHandler<void (mrs_uav_state_estimators::passthrough::Passthrough::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::passthrough::Passthrough*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::passthrough::Passthrough::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::passthrough::Passthrough*)1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::passthrough::Passthrough>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::passthrough::Passthrough::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::passthrough::Passthrough*)1
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<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::passthrough::Passthrough::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::passthrough::Passthrough*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::passthrough::Passthrough::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::passthrough::Passthrough*)::{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::TrackerCommand_<std::allocator<void> > >::lastMsgTime() const7
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()32
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&)32
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&)32
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >&&)32
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&)32
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*)32
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*)32
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<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::HwApiCapabilities_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)32
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<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::HwApiCapabilities_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)32
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::start()32
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&)32
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()32
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&)32
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >&&)32
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::start()32
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&)32
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler()32
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*)32
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*)32
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >&&)32
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()() const32
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()() const32
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()() const32
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<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::HwApiCapabilities_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)::{lambda()#1}::operator()() const32
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()() const32
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()() const32
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()64
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()64
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::getMsg()104
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::hasMsg() const104
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const105
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const105
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*)106
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*)106
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()() const106
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*)109
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*)109
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*)109
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*)109
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&)109
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*)109
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*)109
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*)109
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*)109
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&)109
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*)109
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*)109
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*)109
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*)109
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*)109
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*)109
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::SubscribeHandler()109
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*)109
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*)109
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >&&)109
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::SubscribeHandler()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >&&)109
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::SubscribeHandler()109
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*)109
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*)109
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >&&)109
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::SubscribeHandler()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >&&)109
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::SubscribeHandler()109
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*)109
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*)109
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >&&)109
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::SubscribeHandler()109
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*)109
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*)109
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >&&)109
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::SubscribeHandler()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >&&)109
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >&&)109
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::SubscribeHandler()109
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*)109
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*)109
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >&&)109
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >&&)109
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::SubscribeHandler()109
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*)109
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*)109
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >&&)109
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*)109
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*)109
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*)109
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*)109
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*)109
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*)109
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&)109
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*)109
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*)109
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()() const109
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()() const109
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()() const109
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()() const109
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()() const109
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()() const109
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()() const109
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()() const109
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()() const109
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()() const109
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()() const109
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()() const109
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()() const109
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()() const109
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()() const109
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()() const109
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()() const109
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()() const109
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()() const109
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()() const109
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()() const109
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()() const109
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()() const109
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()() const109
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()() const109
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::hasMsg() const110
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::topicName[abi:cxx11]() const111
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>*)112
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>*)112
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>*)112
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>*)112
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()() const112
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()() const112
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&)113
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >&&)113
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&)113
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()() const113
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&)116
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>*)116
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*)116
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>*)116
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*)116
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*)116
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*)116
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*)116
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*)116
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::start()116
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&)116
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::SubscribeHandler()116
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&)116
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >&&)116
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()() const116
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()() const116
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()() const116
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()() const116
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()() const116
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()() const116
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::start()117
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*)117
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*)117
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()() const117
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const122
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const146
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>*)164
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>*)164
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()() const164
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::start()196
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&)196
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >&&)196
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::lastMsgTime() const215
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::start()218
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&)218
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler()218
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >&&)218
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::start()218
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&)218
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::SubscribeHandler()218
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&)218
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >&&)218
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::~SubscribeHandler()218
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::~SubscribeHandler()218
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::~SubscribeHandler()218
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~SubscribeHandler()218
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::~SubscribeHandler()218
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::~SubscribeHandler()218
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()218
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()218
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::~SubscribeHandler()218
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()218
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::~SubscribeHandler()218
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::start()218
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&)218
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::SubscribeHandler()218
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&)218
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >&&)218
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()() const218
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()() const218
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::start()222
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&)222
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >&&)222
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler()226
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~SubscribeHandler()232
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::start()250
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&)250
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler()250
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >&&)250
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler()250
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >&&)250
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::lastMsgTime() const276
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&)309
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()() const309
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::getMsg()316
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&)327
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&)327
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::start()327
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&)327
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler()327
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >&&)327
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()() const327
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()() const327
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::start()337
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&)337
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >&&)337
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::start()352
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&)352
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >&&)352
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::start()359
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&)359
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::getMsg()380
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::SubscribeHandler()402
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::~SubscribeHandler()402
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler()402
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::~SubscribeHandler()402
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::SubscribeHandler()402
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::~SubscribeHandler()402
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler()403
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::start()421
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&)421
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::SubscribeHandler()421
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&)421
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >&&)421
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()() const421
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler()434
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler()434
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::start()436
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&)436
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >&&)436
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::~SubscribeHandler()436
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::start()436
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&)436
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >&&)436
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~SubscribeHandler()436
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::~SubscribeHandler()436
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::~SubscribeHandler()448
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::~SubscribeHandler()466
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::start()468
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&)468
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::SubscribeHandler()468
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&)468
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >&&)468
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::start()468
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&)468
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()468
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&)468
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >&&)468
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()() const468
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()() const468
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::~SubscribeHandler()500
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler()511
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::~SubscribeHandler()516
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::~SubscribeHandler()520
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::~SubscribeHandler()609
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::~SubscribeHandler()630
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler()634
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::start()650
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&)650
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >&&)650
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::~SubscribeHandler()654
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::getMsg()785
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::getMsg()785
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::hasMsg() const785
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler()838
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler()841
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~SubscribeHandler()842
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler()848
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~SubscribeHandler()936
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()936
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::~SubscribeHandler()986
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::~SubscribeHandler()1185
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~SubscribeHandler()1274
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::~SubscribeHandler()1277
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler()1455
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~SubscribeHandler()2105
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::hasMsg() const2760
mrs_lib::SubscribeHandlerOptions::SubscribeHandlerOptions()2795
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::getMsg()4056
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::getMsg()7214
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::getMsg()10220
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::hasMsg() const10220
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::getMsg()12937
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::hasMsg() const16848
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::hasMsg() const17792
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::hasMsg() const19396
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::hasMsg() const19629
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::getMsg()20521
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::hasMsg() const39903
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::getMsg()40939
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::lastMsgTime() const40939
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::getMsg()50693
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::hasMsg() const58949
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::hasMsg() const74264
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::getMsg()75281
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::hasMsg() const88630
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::hasMsg() const119815
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::hasMsg() const154691
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::getMsg()185781
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::hasMsg() const185787
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::getMsg()210383
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::lastMsgTime() const232848
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::getMsg()250057
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::hasMsg() const271725
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::hasMsg() const436220
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::getMsg()444233
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::lastMsgTime() const464420
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::newMsg() const563084
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::hasMsg() const588098
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::getMsg()638495
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::getMsg()797222
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::hasMsg() const1026589
+
+
+ + + +
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..16f0fe7b69 --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.func.html @@ -0,0 +1,3528 @@ + + + + + + + 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:445186.3 %
Date:2024-12-17 22:35:29Functions:39886246.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> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::start()436
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&)436
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler()841
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*)109
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*)109
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*)106
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<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::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<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::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)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<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>*)112
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::ProcMagDeclination<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcMagDeclination<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::ProcMagDeclination<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcMagDeclination<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::ProcMagDeclination<2>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcMagDeclination<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::ProcMagDeclination<2>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcMagDeclination<2>*)0
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*)109
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*)109
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*)106
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<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::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<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::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)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::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>*)112
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::ProcMagDeclination<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::ProcMagDeclination<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcMagDeclination<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::ProcMagDeclination<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::ProcMagDeclination<2>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcMagDeclination<2>*)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::~SubscribeHandler()1277
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >&&)436
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::start()0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<sensor_msgs::MagneticField_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::SubscribeHandler()402
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::MagneticField_<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::MagneticField_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::MagneticField_<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::MagneticField_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<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::MagneticField_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<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::MagneticField_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::~SubscribeHandler()402
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >&&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::start()32
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::getMsg()10220
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&)32
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler()434
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&)32
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()466
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >&&)32
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::start()218
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&)218
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler()218
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&)109
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*)109
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*)109
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::~SubscribeHandler()436
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >&&)218
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::start()196
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::getMsg()12937
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&)196
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler()434
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&)32
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>*)164
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>*)164
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()630
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >&&)196
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration 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()402
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()402
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> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::stop()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::start()117
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::getMsg()4056
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&)113
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler()403
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>*)112
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>*)112
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::~SubscribeHandler()516
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >&&)113
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::start()352
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::getMsg()210383
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&)352
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler()634
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&)116
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>*)116
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*)116
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>*)116
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*)116
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::~SubscribeHandler()986
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >&&)352
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::start()650
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::getMsg()797222
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&)650
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler()1455
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&)309
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*)109
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*)116
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*)116
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*)109
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*)116
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*)116
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~SubscribeHandler()2105
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >&&)650
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration 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()402
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::~SubscribeHandler()402
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::start()250
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::getMsg()316
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&)250
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler()250
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&)109
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*)109
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*)32
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*)109
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*)32
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::~SubscribeHandler()500
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >&&)250
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::start()222
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::getMsg()185781
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&)222
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler()226
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&)113
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*)109
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*)109
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::~SubscribeHandler()448
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >&&)222
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::start()421
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::getMsg()444233
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&)421
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::SubscribeHandler()421
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&)421
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~SubscribeHandler()842
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >&&)421
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::start()436
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::getMsg()50693
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&)436
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler()838
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&)327
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*)109
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::Float64Stamped_<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::Float64Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::Float64Stamped_<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::Float64Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
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*)109
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<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::Float64Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<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::Float64Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~SubscribeHandler()1274
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >&&)436
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::start()218
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::getMsg()785
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&)218
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::SubscribeHandler()218
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&)218
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~SubscribeHandler()436
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >&&)218
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::start()116
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&)116
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::SubscribeHandler()116
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&)116
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~SubscribeHandler()232
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >&&)116
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::start()109
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::getMsg()785
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::SubscribeHandler()109
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*)109
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*)109
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::~SubscribeHandler()218
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >&&)109
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::start()109
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::getMsg()380
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::SubscribeHandler()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::~SubscribeHandler()218
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >&&)109
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::SubscribeHandler()109
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*)109
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*)109
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::~SubscribeHandler()218
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >&&)109
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::start()359
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::getMsg()20521
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&)359
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler()250
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&)327
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<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::HwApiCapabilities_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)32
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<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::HwApiCapabilities_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)32
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::~SubscribeHandler()609
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >&&)250
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::SubscribeHandler()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~SubscribeHandler()218
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >&&)109
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::SubscribeHandler()109
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*)109
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*)109
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::~SubscribeHandler()218
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >&&)109
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::SubscribeHandler()109
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*)109
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*)109
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::~SubscribeHandler()218
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >&&)109
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::SubscribeHandler()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()218
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >&&)109
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::start()468
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::getMsg()75281
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&)468
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::SubscribeHandler()468
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&)468
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~SubscribeHandler()936
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >&&)468
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration 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> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::start()32
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&)32
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()32
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&)32
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()64
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >&&)32
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()218
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >&&)109
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::start()32
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&)32
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler()32
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*)32
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*)32
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()64
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >&&)32
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::SubscribeHandler()109
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*)109
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*)109
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::~SubscribeHandler()218
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >&&)109
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::start()468
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::getMsg()638495
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&)468
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()468
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&)468
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()936
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >&&)468
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()218
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >&&)109
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::start()109
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&)109
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::SubscribeHandler()109
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*)109
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*)109
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::~SubscribeHandler()218
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >&&)109
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration 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()104
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()511
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()520
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> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::start()327
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::getMsg()7214
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&)327
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler()327
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*)109
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*)109
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*)109
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*)109
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*)109
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*)109
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::~SubscribeHandler()654
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >&&)327
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::start()337
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::getMsg()250057
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&)337
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler()848
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&)109
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*)109
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*)117
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<void (mrs_uav_state_estimators::passthrough::Passthrough::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::passthrough::Passthrough*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::passthrough::Passthrough::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::passthrough::Passthrough*)1
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*)109
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*)117
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<mrs_uav_state_estimators::passthrough::Passthrough>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::passthrough::Passthrough::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::passthrough::Passthrough*)1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::~SubscribeHandler()1185
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >&&)337
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::setNoMessageTimeout(ros::Duration const&)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::start()218
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::getMsg()40939
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&)218
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::SubscribeHandler()218
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&)218
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::~SubscribeHandler()436
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >&&)218
mrs_lib::SubscribeHandlerOptions::SubscribeHandlerOptions()2795
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::getNumPublishers() 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::MagneticField_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::getNumPublishers() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<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> > >::getNumPublishers() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::hasMsg() const10220
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> > >::getNumPublishers() 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> > >::getNumPublishers() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::hasMsg() const16848
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> > >::getNumPublishers() 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> > >::getNumPublishers() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::hasMsg() const17792
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> > >::getNumPublishers() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::hasMsg() const436220
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> > >::getNumPublishers() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::hasMsg() const1026589
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]() const146
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::getNumPublishers() 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() const215
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::hasMsg() const19629
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> > >::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::hasMsg() const185787
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() const464420
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::hasMsg() const119815
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::newMsg() const563084
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> > >::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::hasMsg() const74264
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() const7
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::hasMsg() const785
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() const276
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::hasMsg() const2760
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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::hasMsg() const39903
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]() const111
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::hasMsg() const110
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> > >::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::hasMsg() const88630
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> > >::getNumPublishers() 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> > >::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::hasMsg() const19396
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() const105
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::hasMsg() const154691
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]() const122
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const105
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::getNumPublishers() 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> > >::getNumPublishers() 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> > >::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::hasMsg() const104
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() const232848
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::getNumPublishers() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::hasMsg() const271725
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> > >::getNumPublishers() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::hasMsg() const588098
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]() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::lastMsgTime() const40939
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::getNumPublishers() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::hasMsg() const58949
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()() const109
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()() const109
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()() const106
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<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::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<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::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() 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::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()() const112
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::ProcMagDeclination<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcMagDeclination<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::ProcMagDeclination<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcMagDeclination<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::ProcMagDeclination<2>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcMagDeclination<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::ProcMagDeclination<2>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcMagDeclination<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::MagneticField_<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::MagneticField_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<sensor_msgs::MagneticField_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::MagneticField_<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::MagneticField_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
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()() const32
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()() const109
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()() const109
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()() const32
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()() const164
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()() const112
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()() const116
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()() const116
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()() const116
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()() const309
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()() const109
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()() const116
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()() const116
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()() const109
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()() const109
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()() const32
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()() const113
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()() const109
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()() const421
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()() const327
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()() const109
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::Float64Stamped_<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::Float64Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<mrs_msgs::Float64Stamped_<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::Float64Stamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
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()() const218
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()() const116
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()() const109
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()() const109
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()() const109
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()() const327
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_autostart::automatic_start::AutomaticStart::*)(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<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::HwApiCapabilities_<std::allocator<void> > const>), mrs_uav_autostart::automatic_start::AutomaticStart*)::{lambda()#1}::operator()() const32
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()() const109
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()() const109
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()() const109
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()() const109
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()() const468
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()() const32
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()() const109
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()() const32
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()() const109
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()() const468
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()() const109
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()() const109
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()() const109
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()() const109
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()() const109
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()() const109
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()() const109
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()() const117
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<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::passthrough::Passthrough::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::passthrough::Passthrough*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::passthrough::Passthrough::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::passthrough::Passthrough*)::{lambda()#1}::operator()() const1
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()() const218
+
+
+ + + +
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..af01ffc8cc --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.html @@ -0,0 +1,565 @@ + + + + + + + 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:445186.3 %
Date:2024-12-17 22:35:29Functions:39886246.2 %
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 <optional>
+      11             : 
+      12             : #include <ros/ros.h>
+      13             : #include <mrs_lib/timeout_manager.h>
+      14             : 
+      15             : namespace mrs_lib
+      16             : {
+      17             : 
+      18             :   static const ros::Duration no_timeout = ros::Duration(0);
+      19             : 
+      20             :   /* struct SubscribeHandlerOptions //{ */
+      21             :   
+      22             :   /**
+      23             :   * \brief A helper class to simplify setup of SubscribeHandler construction.
+      24             :   * This class is passed to the SubscribeHandler constructor and specifies its common options.
+      25             :   *
+      26             :   * \note Any option, passed directly to the SubscribeHandler constructor outside this structure, *OVERRIDES* values in this structure.
+      27             :   * The values in this structure can be thought of as default common values for all SubscribeHandler objects you want to create,
+      28             :   * and values passed directly to the constructor as specific options for the concrete handler.
+      29             :   *
+      30             :   */
+      31             :   struct SubscribeHandlerOptions
+      32             :   {
+      33             :     SubscribeHandlerOptions(const ros::NodeHandle& nh) : nh(nh) {}
+      34        2795 :     SubscribeHandlerOptions() = default;
+      35             :   
+      36             :     ros::NodeHandle nh;  /*!< \brief The ROS NodeHandle to be used for subscription. */
+      37             :   
+      38             :     std::string node_name = {};  /*!< \brief Name of the ROS node, using this handle (used for messages printed to console). */
+      39             :   
+      40             :     std::string topic_name = {};  /*!< \brief Name of the ROS topic to be handled. */
+      41             : 
+      42             :     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. */
+      43             :   
+      44             :     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. */
+      45             :   
+      46             :     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. */
+      47             :   
+      48             :     bool threadsafe = true;  /*!< \brief If true, all methods of the SubscribeHandler will be mutexed (using a recursive mutex) to avoid data races. */
+      49             :   
+      50             :     bool autostart = true;  /*!< \brief If true, the SubscribeHandler will be started after construction. Otherwise it has to be started using the start() method */
+      51             :   
+      52             :     uint32_t queue_size = 3;  /*!< \brief This parameter is passed to the NodeHandle when subscribing to the topic */
+      53             :   
+      54             :     ros::TransportHints transport_hints = ros::TransportHints();  /*!< \brief This parameter is passed to the NodeHandle when subscribing to the topic */
+      55             :   };
+      56             :   
+      57             :   //}
+      58             : 
+      59             :   /* SubscribeHandler class //{ */
+      60             :   /**
+      61             :   * \brief The main class for ROS topic subscription, message timeout handling etc.
+      62             :   *
+      63             :   * This class handles the raw ROS Subscriber for a specified topic. The last message received on the topic is remembered
+      64             :   * and may be retrieved by calling the getMsg() method. To check whether at least one message was received, use hasMsg()
+      65             :   * (if no message was received and you call getMsg(), a nullptr is returned and an error message is printed). To check
+      66             :   * whether a new message has arrived since the last call to getMsg(), use newMsg() (useful to check whether a new message
+      67             :   * needs to be processed in a loop or ROS Timer callback).
+      68             :   *
+      69             :   * A timeout callback function may be specified, which is called if no message is received on the topic for a specified
+      70             :   * timeout (use the \p timeout_callback and \p no_message_timeout parameters). If the timeout callback is not set by the
+      71             :   * user, an error message is printed to the console after the timeout by default.
+      72             :   *
+      73             :   * A message callback function may be specified, which is called whenever a new message is received (use the
+      74             :   * \p message_callback parameter).
+      75             :   *
+      76             :   * The callbacks and timeouts may be stopped and started using the stop() and start() methods.
+      77             :   *
+      78             :   * For more details, see the example below (I recommend starting with the simple_example which covers most use-cases).
+      79             :   *
+      80             :   */
+      81             :   template <typename MessageType>
+      82             :   class SubscribeHandler
+      83             :   {
+      84             :     public:
+      85             :     /*!
+      86             :       * \brief Convenience type for the template parameter to enable nice aliasing.
+      87             :       */
+      88             :       using message_type = MessageType;
+      89             : 
+      90             :     /*!
+      91             :       * \brief Type for the timeout callback function.
+      92             :       */
+      93             :       using timeout_callback_t = std::function<void(const std::string& topic_name, const ros::Time& last_msg)>;
+      94             : 
+      95             :     /*!
+      96             :       * \brief Convenience type for the message callback function.
+      97             :       */
+      98             :       using message_callback_t = std::function<void(typename MessageType::ConstPtr)>;
+      99             : 
+     100             :     public:
+     101             :     /*!
+     102             :       * \brief Returns the last received message on the topic, handled by this SubscribeHandler.
+     103             :       * Use hasMsg() first to check if at least one message is available or newMsg() to check if a new message
+     104             :       * since the last call to getMsg() is available.
+     105             :       *
+     106             :       * \return the last received message.
+     107             :       */
+     108     2750435 :       virtual typename MessageType::ConstPtr getMsg() {assert(m_pimpl); return m_pimpl->getMsg();};
+     109             : 
+     110             :     /*!
+     111             :       * \brief Returns the last received message on the topic without modifying the newMsg() or usedMsg() flags.
+     112             :       *
+     113             :       * \return the last received message.
+     114             :       */
+     115           0 :       virtual typename MessageType::ConstPtr peekMsg() const {assert(m_pimpl); return m_pimpl->peekMsg();};
+     116             : 
+     117             :     /*!
+     118             :       * \brief Used to check whether at least one message has been received on the handled topic.
+     119             :       *
+     120             :       * \return true if at least one message was received, otherwise false.
+     121             :       */
+     122     3132337 :       virtual bool hasMsg() const {assert(m_pimpl); return m_pimpl->hasMsg();};
+     123             : 
+     124             :     /*!
+     125             :       * \brief Used to check whether at least one message has been received on the handled topic since the last call to getMsg().
+     126             :       *
+     127             :       * \return true if at least one message was received, otherwise false.
+     128             :       */
+     129      563084 :       virtual bool newMsg() const {assert(m_pimpl); return m_pimpl->newMsg();};
+     130             : 
+     131             :     /*!
+     132             :       * \brief Used to check whether getMsg() was called at least once on this SubscribeHandler.
+     133             :       *
+     134             :       * \return true if getMsg() was called at least once, otherwise false.
+     135             :       */
+     136           0 :       virtual bool usedMsg() const {assert(m_pimpl); return m_pimpl->usedMsg();};
+     137             : 
+     138             :     /*!
+     139             :       * \brief Blocks until new data becomes available or until the timeout runs out or until a spurious wake-up.
+     140             :       *
+     141             :       * \param timeout    after this duration, this method will return a nullptr if no new data becomes available.
+     142             :       * \return           the message if a new message is available after waking up, \p nullptr otherwise.
+     143             :       */
+     144           0 :       virtual typename MessageType::ConstPtr waitForNew(const ros::WallDuration& timeout) {assert(m_pimpl); return m_pimpl->waitForNew(timeout);};
+     145             : 
+     146             :     /*!
+     147             :       * \brief Returns time of the last received message on the topic, handled by this SubscribeHandler.
+     148             :       *
+     149             :       * \return time when the last message was received.
+     150             :       */
+     151      738920 :       virtual ros::Time lastMsgTime() const {assert(m_pimpl); return m_pimpl->lastMsgTime();};
+     152             : 
+     153             :     /*!
+     154             :       * \brief Returns the resolved (full) name of the topic, handled by this SubscribeHandler.
+     155             :       *
+     156             :       * \return name of the handled topic.
+     157             :       */
+     158         379 :       virtual std::string topicName() const {assert(m_pimpl); return m_pimpl->topicName();};
+     159             : 
+     160             :     /*!
+     161             :       * \brief Returns the subscribed (unresolved) name of the topic, handled by this SubscribeHandler.
+     162             :       *
+     163             :       * \return name of the handled topic.
+     164             :       */
+     165           0 :       virtual std::string subscribedTopicName() const {assert(m_pimpl); return m_pimpl->m_topic_name;};
+     166             : 
+     167             :     /*!
+     168             :       * \brief Returns number of publishers registered at the topic.
+     169             :       *
+     170             :       * \return number of publishers.
+     171             :       */
+     172           0 :       virtual uint32_t getNumPublishers() const {assert(m_pimpl); return m_pimpl->getNumPublishers();};
+     173             : 
+     174             :     /*!
+     175             :       * \brief Sets the timeout for no received message.
+     176             :       *
+     177             :       * \param timeout    The new timeout for no received messages.
+     178             :       */
+     179           0 :       virtual void setNoMessageTimeout(const ros::Duration& timeout) {assert(m_pimpl); return m_pimpl->setNoMessageTimeout(timeout);};
+     180             : 
+     181             :     /*!
+     182             :       * \brief Enables the callbacks for the handled topic.
+     183             :       *
+     184             :       * If the SubscribeHandler object is stopped using the stop() method, no callbacks will be called
+     185             :       * until the start() method is called.
+     186             :       */
+     187        7133 :       virtual void start() {assert(m_pimpl); return m_pimpl->start();};
+     188             : 
+     189             :     /*!
+     190             :       * \brief Disables the callbacks for the handled topic.
+     191             :       *
+     192             :       * All messages after this method is called will be ignored until start() is called again.
+     193             :       * Timeout checking will also be disabled.
+     194             :       */
+     195           5 :       virtual void stop() {assert(m_pimpl); return m_pimpl->stop();};
+     196             : 
+     197             :     public:
+     198             :     /*!
+     199             :       * \brief Default constructor to avoid having to use pointers.
+     200             :       *
+     201             :       * It does nothing and the SubscribeHandler it constructs will also do nothing.
+     202             :       * Use some of the other constructors for actual construction of a usable SubscribeHandler.
+     203             :       */
+     204       12047 :       SubscribeHandler() {};
+     205             : 
+     206             :     /*!
+     207             :       * \brief Main constructor.
+     208             :       *
+     209             :       * \param options    The common options struct (see documentation of SubscribeHandlerOptions).
+     210             :       * \param topic_name Name of the topic to be handled by this subscribed.
+     211             :       * \param args       Remaining arguments to be parsed (see other constructors).
+     212             :       *
+     213             :       */
+     214             :       template<class ... Types>
+     215        7129 :       SubscribeHandler(
+     216             :             const SubscribeHandlerOptions& options,
+     217             :             const std::string& topic_name,
+     218             :             Types ... args
+     219             :           )
+     220             :       :
+     221             :         SubscribeHandler(
+     222        7129 :             [options, topic_name]()
+     223             :             {
+     224       14258 :               SubscribeHandlerOptions opts = options;
+     225        7129 :               opts.topic_name = topic_name;
+     226       14258 :               return opts;
+     227             :             }(),
+     228             :             args...
+     229        7129 :             )
+     230             :       {
+     231        7129 :       }
+     232             : 
+     233             :     /*!
+     234             :       * \brief Convenience constructor overload.
+     235             :       *
+     236             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     237             :       * \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).
+     238             :       *
+     239             :       */
+     240        7129 :       SubscribeHandler(
+     241             :             const SubscribeHandlerOptions& options,
+     242             :             const message_callback_t& message_callback = {}
+     243             :           )
+     244        7129 :       {
+     245        7129 :         if (options.threadsafe)
+     246             :         {
+     247        7129 :           m_pimpl = std::make_unique<ImplThreadsafe>
+     248             :             (
+     249             :               options,
+     250             :               message_callback
+     251             :             );
+     252             :         }
+     253             :         else
+     254             :         {
+     255           0 :           m_pimpl = std::make_unique<Impl>
+     256             :             (
+     257             :               options,
+     258             :               message_callback
+     259             :             );
+     260             :         }
+     261        7129 :         if (options.autostart)
+     262        7128 :           start();
+     263        7129 :       };
+     264             : 
+     265             :     /*!
+     266             :       * \brief Convenience constructor overload.
+     267             :       *
+     268             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     269             :       * \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.
+     270             :       * \param args             Remaining arguments to be parsed (see other constructors).
+     271             :       *
+     272             :       */
+     273             :       template <class ... Types>
+     274           1 :       SubscribeHandler(
+     275             :             const SubscribeHandlerOptions& options,
+     276             :             const timeout_callback_t& timeout_callback,
+     277             :             Types ... args
+     278             :           )
+     279             :       :
+     280             :         SubscribeHandler(
+     281           1 :             [options, timeout_callback]()
+     282             :             {
+     283           2 :               SubscribeHandlerOptions opts = options;
+     284           1 :               opts.timeout_callback = timeout_callback;
+     285           2 :               return opts;
+     286             :             }(),
+     287             :             args...
+     288           1 :             )
+     289             :       {
+     290           1 :       }
+     291             : 
+     292             :     /*!
+     293             :       * \brief Convenience constructor overload.
+     294             :       *
+     295             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     296             :       * \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.
+     297             :       * \param obj1             The object on which the callback method \p timeout_callback will be called.
+     298             :       * \param args             Remaining arguments to be parsed (see other constructors).
+     299             :       *
+     300             :       */
+     301             :       template <class ObjectType1, class ... Types>
+     302             :       SubscribeHandler(
+     303             :             const SubscribeHandlerOptions& options,
+     304             :             void (ObjectType1::*const timeout_callback) (const std::string& topic_name, const ros::Time& last_msg),
+     305             :             ObjectType1* const obj1,
+     306             :             Types ... args
+     307             :           )
+     308             :       :
+     309             :         SubscribeHandler(
+     310             :             [options, timeout_callback, obj1]()
+     311             :             {
+     312             :               SubscribeHandlerOptions opts = options;
+     313             :               opts.timeout_callback = timeout_callback == nullptr ? timeout_callback_t() : std::bind(timeout_callback, obj1, std::placeholders::_1, std::placeholders::_2);
+     314             :               return opts;
+     315             :             }(),
+     316             :             args...
+     317             :             )
+     318             :       {
+     319             :       }
+     320             : 
+     321             :     /*!
+     322             :       * \brief Convenience constructor overload.
+     323             :       *
+     324             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     325             :       * \param message_callback The callback method to call when a new message is received.
+     326             :       * \param obj2             The object on which the callback method \p timeout_callback will be called.
+     327             :       * \param args             Remaining arguments to be parsed (see other constructors).
+     328             :       *
+     329             :       */
+     330             :       template <class ObjectType2, class ... Types>
+     331        3059 :       SubscribeHandler(
+     332             :             const SubscribeHandlerOptions& options,
+     333             :             void (ObjectType2::*const message_callback) (typename MessageType::ConstPtr),
+     334             :             ObjectType2* const obj2,
+     335             :             Types ... args
+     336             :           )
+     337             :       :
+     338             :         SubscribeHandler(
+     339             :             options,
+     340        6118 :             message_callback == nullptr ? message_callback_t() : std::bind(message_callback, obj2, std::placeholders::_1),
+     341             :             args...
+     342        3059 :             )
+     343             :       {
+     344        3059 :       }
+     345             : 
+     346             :     /*!
+     347             :       * \brief Convenience constructor overload.
+     348             :       *
+     349             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     350             :       * \param message_callback The callback method to call when a new message is received.
+     351             :       * \param obj2             The object on which the callback method \p timeout_callback will be called.
+     352             :       * \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.
+     353             :       * \param obj1             The object on which the callback method \p timeout_callback will be called.
+     354             :       * \param args             Remaining arguments to be parsed (see other constructors).
+     355             :       *
+     356             :       */
+     357             :      template <class ObjectType1, class ObjectType2, class ... Types>
+     358             :      SubscribeHandler(
+     359             :            const SubscribeHandlerOptions& options,
+     360             :            void (ObjectType2::*const message_callback) (typename MessageType::ConstPtr),
+     361             :            ObjectType2* const obj2,
+     362             :            void (ObjectType1::*const timeout_callback) (const std::string& topic_name, const ros::Time& last_msg),
+     363             :            ObjectType1* const obj1,
+     364             :            Types ... args
+     365             :          )
+     366             :      :
+     367             :        SubscribeHandler(
+     368             :             [options, timeout_callback, obj1]()
+     369             :             {
+     370             :               SubscribeHandlerOptions opts = options;
+     371             :               opts.timeout_callback = timeout_callback == nullptr ? timeout_callback_t() : std::bind(timeout_callback, obj1, std::placeholders::_1, std::placeholders::_2);
+     372             :               return opts;
+     373             :             }(),
+     374             :             message_callback == nullptr ? message_callback_t() : std::bind(message_callback, obj2, std::placeholders::_1),
+     375             :             args...
+     376             :             )
+     377             :      {
+     378             :      }
+     379             : 
+     380             :     /*!
+     381             :       * \brief Convenience constructor overload.
+     382             :       *
+     383             :       * \param options            The common options struct (see documentation of SubscribeHandlerOptions).
+     384             :       * \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.
+     385             :       * \param args               Remaining arguments to be parsed (see other constructors).
+     386             :       *
+     387             :       */
+     388             :       template<class ... Types>
+     389             :       SubscribeHandler(
+     390             :             const SubscribeHandlerOptions& options,
+     391             :             const ros::Duration& no_message_timeout,
+     392             :             Types ... args
+     393             :           )
+     394             :       :
+     395             :         SubscribeHandler(
+     396             :             [options, no_message_timeout]()
+     397             :             {
+     398             :               SubscribeHandlerOptions opts = options;
+     399             :               opts.no_message_timeout = no_message_timeout;
+     400             :               return opts;
+     401             :             }(),
+     402             :             args...
+     403             :             )
+     404             :       {
+     405             :       }
+     406             : 
+     407             :     /*!
+     408             :       * \brief Convenience constructor overload.
+     409             :       *
+     410             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     411             :       * \param timeout_manager  The manager for timeout callbacks.
+     412             :       * \param args             Remaining arguments to be parsed (see other constructors).
+     413             :       *
+     414             :       */
+     415             :       template <class ... Types>
+     416             :       SubscribeHandler(
+     417             :             const SubscribeHandlerOptions& options,
+     418             :             mrs_lib::TimeoutManager& timeout_manager,
+     419             :             Types ... args
+     420             :           )
+     421             :       :
+     422             :         SubscribeHandler(
+     423             :             options,
+     424             :             timeout_manager = timeout_manager,
+     425             :             args...
+     426             :             )
+     427             :       {
+     428             :       }
+     429             : 
+     430       19196 :       ~SubscribeHandler() = default;
+     431             :       // delete copy constructor and assignment operator (forbid copying shandlers)
+     432             :       SubscribeHandler(const SubscribeHandler&) = delete;
+     433             :       SubscribeHandler& operator=(const SubscribeHandler&) = delete;
+     434             :       // define only move constructor and assignemnt operator
+     435          20 :       SubscribeHandler(SubscribeHandler&& other)
+     436          20 :       {
+     437          20 :         this->m_pimpl = std::move(other.m_pimpl);
+     438          20 :         other.m_pimpl = nullptr;
+     439          20 :       }
+     440        7000 :       SubscribeHandler& operator=(SubscribeHandler&& other)
+     441             :       {
+     442        7000 :         this->m_pimpl = std::move(other.m_pimpl);
+     443        7000 :         other.m_pimpl = nullptr;
+     444        7000 :         return *this;
+     445             :       }
+     446             : 
+     447             :     private:
+     448             :       class Impl;
+     449             :       class ImplThreadsafe;
+     450             :       std::unique_ptr<Impl> m_pimpl;
+     451             :   };
+     452             :   //}
+     453             : 
+     454             : /*!
+     455             :   * \brief Helper alias for convenient extraction of handled message type from a SubscribeHandlerPtr.
+     456             :   */
+     457             :   template<typename SubscribeHandler>
+     458             :   using message_type = typename SubscribeHandler::message_type;
+     459             : 
+     460             : /*!
+     461             :   * \brief Helper function for object construstion e.g. in case of member objects.
+     462             :   * This function is useful to avoid specifying object template parameters twice - once in definition of the variable and second time during object construction.
+     463             :   * 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
+     464             :   * argument and not as a return type.
+     465             :   *
+     466             :   * \param object The object to be constructed.
+     467             :   * \param args   These arguments are passed to the object constructor.
+     468             :   */
+     469             :   template<typename Class, class ... Types>
+     470             :   void construct_object(
+     471             :         Class& object,
+     472             :         Types ... args
+     473             :       )
+     474             :   {
+     475             :     object = Class(args...);
+     476             :   }
+     477             : }
+     478             : 
+     479             : #include <mrs_lib/impl/subscribe_handler.hpp>
+     480             : 
+     481             : #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..5b1ea6c673 --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.overview.html @@ -0,0 +1,141 @@ + + + + + + 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 + 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..6d832aa244e69a4c92ffb96c56ec0c4d657f6a3d GIT binary patch literal 1846 zcmV-62g&$}P)ll1cJ@3z3JX{y)p}WOlK%gbBZ;N`X_>CITR6} zp)aS8V6qMDPezo7>X#Umer90y5FKLmp!a54Xlm zqd|ofLWlaOQX3>j2!pWZ%&|T~nJGcu^l4-clRj}~Qbr!oxH?P20t`~wo%mTw3D!5>C!y}Imf=4Rb0-_}7(Rd;}D!DW#eX3WV zS62X$sg`%(w!2YYcxd6@SwfoB)wpMe&Bfd!lYX=4JHp`=wMxyADQ-i}3bm`Fhqs_8 zm^OlTjpgwP9gh@iBs8rKUP3$44pdTHIo%Xdy)B7*E+r|omsIB#@2(VZX`qTVF4)L) z;6ehnQ7FxiW4#((W;c#kF2toGW75-VT!9_Z5HIogC`@9>$H=ZM#cj;g=+`L z3j-JX-WBv|M1E9r6R`pxh|ym)oV=N==N|0iBnR#!2{H(-KDxG3d4QWzY6zF^$mA!@ zNwk%+8^Uaz1FSzhNIXM#i~#vhhr6F3ucf-~!qbIN3+f~1m9X|eZUmR5WnKy1>Ic`Q zU{@v!GC(6Vv%Jmgv6NTP)TX05!~9qk^lHqAHdv)Jtxu%0)8 zhNhpFq(rpvw)L4q_bdlIRBIv5a^;o}y;OoDtWnP1MVmioN_hmW?L|`0NmBD=1yX~g z_?7DBuOg{|2;qnTE}Ql4jaD!sI4BriRf<_-9-{{k!0(_Gx$gkYwz!2;-=16p&DUg)k?N40FJv#sE ztM=C_uG*f4QmjQB-4`1;?sgiVc=vj`j8aOerh%VADIT4Y`yIjGkgl`ZU2lKW9?!XM zd{f-|c;h31oBCaqRw5@Oy6rswaq}GI?sXKT6b>jCYSB9vX9|3>uye_Ie!V$)sBMGn z1bZ6IrnL{{hRRY3KG2zY*P(`>t&fgo&gp5qCewlzPLTj_uSF*h)t@~JAO2?L{^G2Qg#&%NozbDU5ilhfNG1PO-Q}4D zvjE=S*$f}QGq@uJE&QmM;VK3{pf+p}NULc*)}1r1Vz1Yt6fANNEj_kd0^ibjdJ$gv k4^;jL!H4%E#sB~S07*qoM6N<$g2`-Q!2kdN 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..d5fb456a37 --- /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:4580.0 %
Date:2024-12-17 22:35:29Functions: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..5f6a205d82 --- /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:4580.0 %
Date:2024-12-17 22:35:29Functions: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..3f53fa9854 --- /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:4580.0 %
Date:2024-12-17 22:35:29Functions: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          85 :       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      143600 :       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          89 :       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           0 :       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..06b4052eb434cb7998075b948b54341d97910cdf GIT binary patch literal 454 zcmV;%0XhDOP)4V z=>e|>HYQiL1cCKVf=Mj>3q{+M^)&L;620h5XdLR`QnPuS@L%gkxg4TRDGN|#X&J|H z_A<@$Zehb(9w(;=i6Bl2$~r6?Hmr?ya76F5Q4V7Lyyo?{INqan#_@#NybCzSaaopi zeXLHN{=Pjt;*M>2llQ!3+5$=(f+$^^kVqO9IpE`8`y`S@0%*D@Fk z)=nD~BTYn69IY!7;M`q-a+857DLlH1 wm0T6EedrYcJO8=I$2w@Z`^SgxJIGhX3q&Y$5X87!M*si-07*qoM6N<$g6reK@c;k- literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/timer.h.func-sort-c.html b/mrs_lib/include/mrs_lib/timer.h.func-sort-c.html new file mode 100644 index 0000000000..75b89f2cc8 --- /dev/null +++ b/mrs_lib/include/mrs_lib/timer.h.func-sort-c.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-12-17 22:35:29Functions: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..d2e12e7697 --- /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-12-17 22:35:29Functions: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..3f53afc242 --- /dev/null +++ b/mrs_lib/include/mrs_lib/timer.h.gcov.html @@ -0,0 +1,353 @@ + + + + + + + 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-12-17 22:35:29Functions: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             :   /**
+      13             :    * @brief Common wrapper representing the functionality of the ros::Timer.
+      14             :    *
+      15             :    * The implementation can then use either ros::Timer (the ROSTimer class)
+      16             :    * or threads and synchronization primitives from the C++ standard library
+      17             :    * (the ThreadTimer class). Both these variants implement the same interface.
+      18             :    *
+      19             :    * @note Functionality of the two implementations differs in some details.
+      20             :    */
+      21             :   class MRSTimer
+      22             :   {
+      23             :     public:
+      24             :     using callback_t = std::function<void(const ros::TimerEvent&)>;
+      25             : 
+      26             :     /**
+      27             :      * @brief stop the timer
+      28             :      */
+      29             :     virtual void stop() = 0;
+      30             : 
+      31             :     /**
+      32             :      * @brief start the timer
+      33             :      */
+      34             :     virtual void start() = 0;
+      35             : 
+      36             :     /**
+      37             :      * @brief set the timer period/duration
+      38             :      *
+      39             :      * @param duration
+      40             :      * @param reset
+      41             :      */
+      42             :     virtual void setPeriod(const ros::Duration& duration, const bool reset = true) = 0;
+      43             : 
+      44             :     /**
+      45             :      * @brief change the callback method 
+      46             :      *
+      47             :      * Usable e.g. for running thread with a specific parameter if you bind it using std::bind
+      48             :      *
+      49             :      * @param callback          callback method to be called.
+      50             :      */
+      51             :     virtual void setCallback(const std::function<void(const ros::TimerEvent&)>& callback) = 0;
+      52             : 
+      53             :     /**
+      54             :      * @brief returns true if callbacks should be called
+      55             :      *
+      56             :      * @return true if timer is running
+      57             :      */
+      58             :     virtual bool running() = 0;
+      59             : 
+      60          16 :     virtual ~MRSTimer() = default;
+      61             :     MRSTimer(const MRSTimer&) = default;
+      62             :     MRSTimer(MRSTimer&&) = default;
+      63             :     MRSTimer& operator=(const MRSTimer&) = default;
+      64             :     MRSTimer& operator=(MRSTimer&&) = default;
+      65             : 
+      66             :     protected:
+      67          16 :     MRSTimer() = default;
+      68             :   };
+      69             : 
+      70             :   // | ------------------------ ROSTimer ------------------------ |
+      71             : 
+      72             :   /* class ROSTimer //{ */
+      73             : 
+      74             :   /**
+      75             :    * @brief ros::Timer wrapper. The interface is the same as with ros::Timer, except for the initialization method.
+      76             :    */
+      77             :   class ROSTimer : public MRSTimer {
+      78             :   public:
+      79             :     ROSTimer();
+      80             : 
+      81             :     /**
+      82             :      * @brief Constructs the object.
+      83             :      *
+      84             :      * @tparam ObjectType
+      85             :      * @param nh                            ROS node handle to be used for creating the underlying ros::Timer object.
+      86             :      * @param rate                          rate at which the callback should be called.
+      87             :      * @param ObjectType::*const callback   callback method to be called.
+      88             :      * @param obj                           object for the method.
+      89             :      * @param oneshot                       whether the callback should only be called once after starting.
+      90             :      * @param autostart                     whether the timer should immediately start after construction.
+      91             :      */
+      92             :     template <class ObjectType>
+      93             :     ROSTimer(const ros::NodeHandle& nh, const ros::Rate& rate, void (ObjectType::*const callback)(const ros::TimerEvent&), ObjectType* const obj,
+      94             :              const bool oneshot = false, const bool autostart = true);
+      95             : 
+      96             :     /**
+      97             :      * @brief full constructor
+      98             :      *
+      99             :      * @tparam ObjectType
+     100             :      * @param nh                            ROS node handle to be used for creating the underlying ros::Timer object.
+     101             :      * @param duration                      desired callback period.
+     102             :      * @param ObjectType::*const callback   callback method to be called.
+     103             :      * @param obj                           object for the method.
+     104             :      * @param oneshot                       whether the callback should only be called once after starting.
+     105             :      * @param autostart                     whether the timer should immediately start after construction.
+     106             :      */
+     107             :     template <class ObjectType>
+     108             :     ROSTimer(const ros::NodeHandle& nh, const ros::Duration& duration, void (ObjectType::*const callback)(const ros::TimerEvent&), ObjectType* const obj,
+     109             :              const bool oneshot = false, const bool autostart = true);
+     110             : 
+     111             :     /**
+     112             :      * @brief stop the timer
+     113             :      */
+     114             :     virtual void stop() override;
+     115             : 
+     116             :     /**
+     117             :      * @brief start the timer
+     118             :      */
+     119             :     virtual void start() override;
+     120             : 
+     121             :     /**
+     122             :      * @brief set the timer period/duration
+     123             :      *
+     124             :      * @param duration
+     125             :      * @param reset
+     126             :      */
+     127             :     virtual void setPeriod(const ros::Duration& duration, const bool reset = true) override;
+     128             : 
+     129             :     /**
+     130             :      * @brief change the callback method
+     131             :      *
+     132             :      * Usable e.g. for running thread with a specific parameter if you bind it using std::bind
+     133             :      *
+     134             :      * @param callback          callback method to be called.
+     135             :      */
+     136             :     virtual void setCallback(const std::function<void(const ros::TimerEvent&)>& callback) override;
+     137             : 
+     138             :     /**
+     139             :      * @brief returns true if callbacks should be called
+     140             :      *
+     141             :      * @return true if timer is running
+     142             :      */
+     143             :     virtual bool running() override;
+     144             : 
+     145          16 :     virtual ~ROSTimer() override {stop();};
+     146             :     // to keep rule of five since we have a custom destructor
+     147             :     ROSTimer(const ROSTimer&) = delete;
+     148             :     ROSTimer(ROSTimer&&) = delete;
+     149             :     ROSTimer& operator=(const ROSTimer&) = delete;
+     150             :     ROSTimer& operator=(ROSTimer&&) = delete;
+     151             : 
+     152             :   private:
+     153             :     std::mutex mutex_timer_;
+     154             : 
+     155             :     std::unique_ptr<ros::Timer> timer_;
+     156             :   };
+     157             : 
+     158             :   //}
+     159             : 
+     160             :   // | ----------------------- ThreadTimer ---------------------- |
+     161             : 
+     162             :   /* class ThreadTimer //{ */
+     163             : 
+     164             :   /**
+     165             :    * @brief Custom thread-based Timers with the same interface as mrs_lib::ROSTimer.
+     166             :    */
+     167             :   class ThreadTimer : public MRSTimer {
+     168             : 
+     169             :   public:
+     170             :     ThreadTimer();
+     171             : 
+     172             :     /**
+     173             :      * @brief Constructs the object.
+     174             :      *
+     175             :      * @tparam ObjectType
+     176             :      * @param nh                            ignored (used just for consistency with ROSTimer)
+     177             :      * @param rate                          rate at which the callback should be called.
+     178             :      * @param ObjectType::*const callback   callback method to be called.
+     179             :      * @param obj                           object for the method.
+     180             :      * @param oneshot                       whether the callback should only be called once after starting.
+     181             :      * @param autostart                     whether the timer should immediately start after construction.
+     182             :      */
+     183             :     template <class ObjectType>
+     184             :     ThreadTimer(const ros::NodeHandle& nh, const ros::Rate& rate, void (ObjectType::*const callback)(const ros::TimerEvent&), ObjectType* const obj,
+     185             :                 const bool oneshot = false, const bool autostart = true);
+     186             : 
+     187             :     /**
+     188             :      * @brief Constructs the object.
+     189             :      *
+     190             :      * @tparam ObjectType
+     191             :      * @param nh                            ignored (used just for consistency with ROSTimer)
+     192             :      * @param duration                      desired callback period.
+     193             :      * @param ObjectType::*const callback   callback method to be called.
+     194             :      * @param obj                           object for the method.
+     195             :      * @param oneshot                       whether the callback should only be called once after starting.
+     196             :      * @param autostart                     whether the timer should immediately start after construction.
+     197             :      */
+     198             :     template <class ObjectType>
+     199             :     ThreadTimer(const ros::NodeHandle& nh, const ros::Duration& duration, void (ObjectType::*const callback)(const ros::TimerEvent&), ObjectType* const obj,
+     200             :                 bool oneshot = false, const bool autostart = true);
+     201             : 
+     202             :     /**
+     203             :      * @brief stop the timer
+     204             :      *
+     205             :      * No more callbacks will be called after this method returns.
+     206             :      */
+     207             :     virtual void stop() override;
+     208             : 
+     209             :     /**
+     210             :      * @brief start the timer
+     211             :      *
+     212             :      * The first callback will be called in now + period.
+     213             :      *
+     214             :      * @note If the timer is already started, nothing will change.
+     215             :      */
+     216             :     virtual void start() override;
+     217             : 
+     218             :     /**
+     219             :      * @brief set the timer period/duration
+     220             :      *
+     221             :      * The new period will be applied after the next callback.
+     222             :      *
+     223             :      * @param duration the new desired callback period.
+     224             :      * @param reset    ignored in this implementation.
+     225             :      */
+     226             :     virtual void setPeriod(const ros::Duration& duration, const bool reset = true) override;
+     227             :     
+     228             :     /**
+     229             :      * @brief change the callback method
+     230             :      *
+     231             :      * Usable e.g. for running thread with a specific parameter if you bind it using std::bind
+     232             :      *
+     233             :      * @param callback          callback method to be called.
+     234             :      */
+     235             :     virtual void setCallback(const std::function<void(const ros::TimerEvent&)>& callback) override;
+     236             : 
+     237             :     /**
+     238             :      * @brief returns true if callbacks should be called
+     239             :      *
+     240             :      * @return true if timer is running
+     241             :      */
+     242             :     virtual bool running() override;
+     243             : 
+     244             :     /**
+     245             :      * @brief stops the timer and then destroys the object
+     246             :      *
+     247             :      * No more callbacks will be called when the destructor is started.
+     248             :      */
+     249          16 :     virtual ~ThreadTimer() override {stop();};
+     250             :     // to keep rule of five since we have a custom destructor
+     251             :     ThreadTimer(const ThreadTimer&) = delete;
+     252             :     ThreadTimer(ThreadTimer&&) = delete;
+     253             :     ThreadTimer& operator=(const ThreadTimer&) = delete;
+     254             :     ThreadTimer& operator=(ThreadTimer&&) = delete;
+     255             : 
+     256             :   private:
+     257             :     class Impl;
+     258             : 
+     259             :     std::unique_ptr<Impl> impl_;
+     260             : 
+     261             :   };  // namespace mrs_lib
+     262             : 
+     263             :   //}
+     264             : 
+     265             : #include <mrs_lib/impl/timer.hpp>
+     266             : 
+     267             : }  // namespace mrs_lib
+     268             : 
+     269             : #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..6e9faf4abc --- /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..ae79f7e763795eb77fe95f163a91ff757059b55a GIT binary patch literal 927 zcmV;Q17Q4#P)r?;jj;q>j$cY94F#$<*Pdz-dBZaRZLT1~B>uz`NjKQ0V{@O*jD0 zl*E2KUa!~s+SVE#kK@IJ+j{nCSx50+zg^Kz#omU}`&)rFop_x^HV)D1=VtWUfQI2| z0k?<8tm6JuEq?HOKld-6I^ifQpi55bviyBdV%$thd%f`2mQ#)+_;jZRZ|ym^h((vb zNhcJlmh@Or$C5BmmLR0kToP)z6e1^y#yz1-8*GfD;^sl3 zw@H~xT4Wp>A|PUr{DfE#VCS6gh<10)XXWqnVOHp;3t(-`9cBpy?h90semczFFr3Y* z(f`J9c9w@$j`whO4`(eM@8PV;@qNSDjKtQ%SsuNPhqLFp{eKOA{8;xpmh$+(p4Hj4 z(7(pk-pAgCGEDl?P0Nn44%aIZD~>w&8ykr1?~jk(G6<2=9?p5f)V!QevhKN!gU!QYZ+* zSEFu;*B6G_J(9Kw8)26FaZu!wVr+TKNNN=gkefXuZA}#_-!&CdV;002ovPDHLkV1h50 Bt$_dl literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/transformer.h.func-sort-c.html b/mrs_lib/include/mrs_lib/transformer.h.func-sort-c.html new file mode 100644 index 0000000000..285bacfd95 --- /dev/null +++ b/mrs_lib/include/mrs_lib/transformer.h.func-sort-c.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-12-17 22:35:29Functions: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&)339
mrs_lib::Transformer::retryLookupNewest(bool)546
mrs_lib::Transformer::resolveFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1682
mrs_lib::Transformer::setDefaultFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)506350
mrs_lib::Transformer::frame_from[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)2120895
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&)2120961
mrs_lib::Transformer::frame_to[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)2123955
mrs_lib::Transformer::frame_from[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> > const&)2133396
mrs_lib::Transformer::frame_to[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> > const&)2133408
+
+
+ + + +
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..388969d42a --- /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-12-17 22:35:29Functions: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&)2133396
mrs_lib::Transformer::frame_from[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)2120895
mrs_lib::Transformer::resolveFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1682
mrs_lib::Transformer::setDefaultFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)506350
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&)2120961
mrs_lib::Transformer::setDefaultPrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)339
mrs_lib::Transformer::setLookupTimeout(ros::Duration)2
mrs_lib::Transformer::retryLookupNewest(bool)546
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&)2133408
mrs_lib::Transformer::frame_to[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)2123955
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..8b59180d5a --- /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-12-17 22:35:29Functions: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      506350 :     void setDefaultFrame(const std::string& frame_id)
+     131             :     {
+     132     1012353 :       std::scoped_lock lck(mutex_);
+     133      506241 :       default_frame_id_ = frame_id;
+     134      506197 :     }
+     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         339 :     void setDefaultPrefix(const std::string& prefix)
+     154             :     {
+     155         678 :       std::scoped_lock lck(mutex_);
+     156         339 :       if (prefix.empty())
+     157           1 :         prefix_ = "";
+     158             :       else
+     159         338 :         prefix_ = prefix + "/";
+     160         339 :     }
+     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         546 :     void retryLookupNewest(const bool retry = true)
+     212             :     {
+     213         546 :       std::scoped_lock lck(mutex_);
+     214         546 :       retry_lookup_newest_ = retry;
+     215         546 :     }
+     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        1682 :     std::string resolveFrame(const std::string& frame_id)
+     249             :     {
+     250        3364 :       std::scoped_lock lck(mutex_);
+     251        3364 :       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     2133396 :     static constexpr const std::string& frame_from(const geometry_msgs::TransformStamped& msg)
+     551             :     {
+     552     2133396 :       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     2120895 :     static constexpr std::string& frame_from(geometry_msgs::TransformStamped& msg)
+     564             :     {
+     565     2120895 :       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     2133408 :     static constexpr const std::string& frame_to(const geometry_msgs::TransformStamped& msg)
+     575             :     {
+     576     2133408 :       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     2123955 :     static constexpr std::string& frame_to(geometry_msgs::TransformStamped& msg)
+     588             :     {
+     589     2123955 :       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     2120961 :     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     2120961 :       geometry_msgs::TransformStamped ret;
+     621     2120855 :       frame_from(ret) = from_frame;
+     622     2120959 :       frame_to(ret) = to_frame;
+     623     2120944 :       ret.header.stamp = time_stamp;
+     624     2120944 :       ret.transform = tf;
+     625     2120944 :       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-12-17 22:35:29Functions: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..68f6e70520 --- /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-12-17 22:35:29Functions: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..03bdfe5b86 --- /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-12-17 22:35:29Functions: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..1a83ac1dcf --- /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-12-17 22:35:29Functions: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)47828
+
+
+ + + +
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..0e870bb0c7 --- /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-12-17 22:35:29Functions: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)47828
+
+
+ + + +
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..5bfd1973a5 --- /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-12-17 22:35:29Functions: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       47828 :   int signum(T val)
+     139             :   {
+     140       47828 :     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-12-17 22:35:29Functions: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..f4301928bb --- /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-12-17 22:35:29Functions: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..a2802aa459 --- /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-12-17 22:35:29Functions: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..008a2df1ec --- /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-12-17 22:35:29Functions: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&) const272
+
+
+ + + +
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..3d955a04dc --- /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-12-17 22:35:29Functions: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&) const272
+
+
+ + + +
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..eaafde9c0e --- /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-12-17 22:35:29Functions: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         272 :   bool operator<(const VisualObject& other) const {
+      73         272 :     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..e720798cdb --- /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-12-17 22:35:29Functions: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&)22779
mrs_lib::AttitudeConverter::getYawRateIntrinsic(double const&)115030
mrs_lib::AttitudeConverter::operator std::tuple<double&, double&, double&>()154140
mrs_lib::AttitudeConverter::getYaw()206284
mrs_lib::AttitudeConverter::calculateRPY()206289
mrs_lib::Vector3Converter::Vector3Converter(geometry_msgs::Vector3_<std::allocator<void> > const&)321073
mrs_lib::AttitudeConverter::setHeading(double const&)323426
mrs_lib::AttitudeConverter::getVectorZ()325424
mrs_lib::AttitudeConverter::getHeadingRate(mrs_lib::Vector3Converter const&)343853
mrs_lib::AttitudeConverter::operator tf2::Matrix3x3() const426083
mrs_lib::AttitudeConverter::operator tf2::Transform() const438920
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Quaternion<double, 0>)507096
mrs_lib::Vector3Converter::operator Eigen::Matrix<double, 3, 1, 0, 3, 1>() const669271
mrs_lib::AttitudeConverter::operator tf2::Quaternion() const986831
mrs_lib::AttitudeConverter::AttitudeConverter(tf2::Quaternion)1051507
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Matrix<double, 3, 3, 0, 3, 3>)1574936
mrs_lib::AttitudeConverter::operator Eigen::Matrix<double, 3, 3, 0, 3, 3>() const1672397
mrs_lib::AttitudeConverter::getHeading()1794046
mrs_lib::AttitudeConverter::AttitudeConverter(geometry_msgs::Quaternion_<std::allocator<void> >)3952593
mrs_lib::AttitudeConverter::AttitudeConverter(double const&, double const&, double const&, mrs_lib::RPY_convention_t const&)7921396
mrs_lib::AttitudeConverter::operator geometry_msgs::Quaternion_<std::allocator<void> >() const9477776
mrs_lib::AttitudeConverter::validateOrientation()15005628
+
+
+ + + +
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..221fdc3cc3 --- /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-12-17 22:35:29Functions: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&)321073
mrs_lib::Vector3Converter::Vector3Converter(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)22779
mrs_lib::Vector3Converter::Vector3Converter(double const&, double const&, double const&)1
mrs_lib::AttitudeConverter::getHeading()1794046
mrs_lib::AttitudeConverter::getVectorX()3
mrs_lib::AttitudeConverter::getVectorY()3
mrs_lib::AttitudeConverter::getVectorZ()325424
mrs_lib::AttitudeConverter::setHeading(double const&)323426
mrs_lib::AttitudeConverter::calculateRPY()206289
mrs_lib::AttitudeConverter::getHeadingRate(mrs_lib::Vector3Converter const&)343853
mrs_lib::AttitudeConverter::getExtrinsicRPY()1
mrs_lib::AttitudeConverter::getIntrinsicRPY()1
mrs_lib::AttitudeConverter::getYawRateIntrinsic(double const&)115030
mrs_lib::AttitudeConverter::validateOrientation()15005628
mrs_lib::AttitudeConverter::getYaw()206284
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> >)3952593
mrs_lib::AttitudeConverter::AttitudeConverter(tf::Quaternion)1
mrs_lib::AttitudeConverter::AttitudeConverter(tf2::Quaternion)1051507
mrs_lib::AttitudeConverter::AttitudeConverter(tf2::Matrix3x3)1
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Quaternion<double, 0>)507096
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Matrix<double, 3, 3, 0, 3, 3>)1574936
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&)7921396
mrs_lib::AttitudeConverter::operator std::tuple<double&, double&, double&>()154140
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>() const669271
mrs_lib::AttitudeConverter::operator geometry_msgs::Quaternion_<std::allocator<void> >() const9477776
mrs_lib::AttitudeConverter::operator tf::Quaternion() const1
mrs_lib::AttitudeConverter::operator tf2::Quaternion() const986831
mrs_lib::AttitudeConverter::operator tf2::Matrix3x3() const426083
mrs_lib::AttitudeConverter::operator tf2::Transform() const438920
mrs_lib::AttitudeConverter::operator Eigen::Matrix<double, 3, 3, 0, 3, 3>() const1672397
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..7c747c21f9 --- /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-12-17 22:35:29Functions: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       22779 : Vector3Converter::Vector3Converter(const Eigen::Vector3d& vector3) {
+      33             : 
+      34       22779 :   vector3_[0] = vector3[0];
+      35       22779 :   vector3_[1] = vector3[1];
+      36       22779 :   vector3_[2] = vector3[2];
+      37       22779 : }
+      38             : 
+      39      321073 : Vector3Converter::Vector3Converter(const geometry_msgs::Vector3& vector3) {
+      40             : 
+      41      321046 :   vector3_[0] = vector3.x;
+      42      321049 :   vector3_[1] = vector3.y;
+      43      320986 :   vector3_[2] = vector3.z;
+      44      321010 : }
+      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      669271 : Vector3Converter::operator Eigen::Vector3d() const {
+      59             : 
+      60      669271 :   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     7921396 : AttitudeConverter::AttitudeConverter(const double& roll, const double& pitch, const double& yaw, const RPY_convention_t& format) {
+      79             : 
+      80     7920506 :   switch (format) {
+      81             : 
+      82     7920483 :     case RPY_EXTRINSIC: {
+      83     7920483 :       tf2_quaternion_.setRPY(roll, pitch, yaw);
+      84     7920546 :       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     7920569 :   validateOrientation();
+     104     7920756 : }
+     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     3952593 : AttitudeConverter::AttitudeConverter(const geometry_msgs::Quaternion quaternion) {
+     117     3951802 :   tf2_quaternion_.setX(quaternion.x);
+     118     3951078 :   tf2_quaternion_.setY(quaternion.y);
+     119     3952220 :   tf2_quaternion_.setZ(quaternion.z);
+     120     3952552 :   tf2_quaternion_.setW(quaternion.w);
+     121             : 
+     122     3952683 :   validateOrientation();
+     123     3951504 : }
+     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      507096 : AttitudeConverter::AttitudeConverter(const Eigen::Quaterniond quaternion) {
+     132      507096 :   tf2_quaternion_.setX(quaternion.x());
+     133      507096 :   tf2_quaternion_.setY(quaternion.y());
+     134      507095 :   tf2_quaternion_.setZ(quaternion.z());
+     135      507095 :   tf2_quaternion_.setW(quaternion.w());
+     136             : 
+     137      507094 :   validateOrientation();
+     138      507096 : }
+     139             : 
+     140     1574936 : AttitudeConverter::AttitudeConverter(const Eigen::Matrix3d matrix) {
+     141     1572035 :   Eigen::Quaterniond quaternion = Eigen::Quaterniond(matrix);
+     142             : 
+     143     1573636 :   tf2_quaternion_.setX(quaternion.x());
+     144     1571877 :   tf2_quaternion_.setY(quaternion.y());
+     145     1573329 :   tf2_quaternion_.setZ(quaternion.z());
+     146     1573156 :   tf2_quaternion_.setW(quaternion.w());
+     147             : 
+     148     1573117 :   validateOrientation();
+     149     1570296 : }
+     150             : 
+     151     1051507 : AttitudeConverter::AttitudeConverter(const tf2::Quaternion quaternion) {
+     152             : 
+     153     1050067 :   tf2_quaternion_ = quaternion;
+     154             : 
+     155     1050067 :   validateOrientation();
+     156     1050181 : }
+     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      986831 : AttitudeConverter::operator tf2::Quaternion() const {
+     170      986831 :   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     9477776 : AttitudeConverter::operator geometry_msgs::Quaternion() const {
+     186             : 
+     187     9477776 :   geometry_msgs::Quaternion geom_quaternion;
+     188             : 
+     189     9474491 :   geom_quaternion.x = tf2_quaternion_.x();
+     190     9470107 :   geom_quaternion.y = tf2_quaternion_.y();
+     191     9471055 :   geom_quaternion.z = tf2_quaternion_.z();
+     192     9471946 :   geom_quaternion.w = tf2_quaternion_.w();
+     193             : 
+     194     9471868 :   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     1672397 : AttitudeConverter::operator Eigen::Matrix3d() const {
+     206             : 
+     207     1672397 :   Eigen::Quaterniond quaternion(tf2_quaternion_.w(), tf2_quaternion_.x(), tf2_quaternion_.y(), tf2_quaternion_.z());
+     208             : 
+     209     3344509 :   return quaternion.toRotationMatrix();
+     210             : }
+     211             : 
+     212      154140 : AttitudeConverter::operator std::tuple<double&, double&, double&>() {
+     213             : 
+     214      154140 :   tf2::Matrix3x3(tf2_quaternion_).getRPY(roll_, pitch_, yaw_);
+     215             : 
+     216      154140 :   return std::tuple<double&, double&, double&>{roll_, pitch_, yaw_};
+     217             : }
+     218             : 
+     219      426083 : AttitudeConverter::operator tf2::Matrix3x3() const {
+     220             : 
+     221      426083 :   return tf2::Matrix3x3(tf2_quaternion_);
+     222             : }
+     223             : 
+     224      438920 : AttitudeConverter::operator tf2::Transform() const {
+     225             : 
+     226      438920 :   return tf2::Transform(tf2_quaternion_);
+     227             : }
+     228             : 
+     229             : //}
+     230             : 
+     231             : /* getters //{ */
+     232             : 
+     233      206284 : double AttitudeConverter::getYaw(void) {
+     234             : 
+     235      206284 :   calculateRPY();
+     236             : 
+     237      206284 :   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     1794046 : double AttitudeConverter::getHeading(void) {
+     255             : 
+     256     1794046 :   tf2::Vector3 b1 = tf2::Vector3(1, 0, 0);
+     257             : 
+     258     1793252 :   tf2::Vector3 x_new = tf2::Transform(tf2_quaternion_) * b1;
+     259             : 
+     260     1792842 :   if (fabs(x_new[0]) <= 1e-3 && fabs(x_new[1]) <= 1e-3) {
+     261           3 :     throw GetHeadingException();
+     262             :   }
+     263             : 
+     264     3582277 :   return atan2(x_new[1], x_new[0]);
+     265             : }
+     266             : 
+     267      115030 : 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      115030 :   if (fabs(heading_rate) < 1e-3) {
+     272       83443 :     return 0;
+     273             :   }
+     274             : 
+     275             :   // prep
+     276       31587 :   Eigen::Matrix3d R = *this;
+     277             : 
+     278             :   // construct the heading orbital velocity vector
+     279       31587 :   Eigen::Vector3d heading_vector   = Eigen::Vector3d(R(0, 0), R(1, 0), 0);
+     280       31587 :   Eigen::Vector3d orbital_velocity = Eigen::Vector3d(0, 0, heading_rate).cross(heading_vector);
+     281             : 
+     282             :   // projector to the heading orbital velocity vector subspace
+     283       31587 :   Eigen::Vector3d b_orb = Eigen::Vector3d(0, 0, 1).cross(heading_vector);
+     284       31587 :   b_orb.normalize();
+     285       31587 :   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       31587 :   Eigen::Vector3d projected = P * R.col(1);
+     289             : 
+     290             : 
+     291       31587 :   double orbital_velocity_norm = orbital_velocity.norm();
+     292       31587 :   double projected_norm        = projected.norm();
+     293             : 
+     294       31587 :   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       31587 :   double direction = mrs_lib::signum(orbital_velocity.dot(projected));
+     300             : 
+     301       31587 :   double output_yaw_rate = direction * (orbital_velocity_norm / projected_norm);
+     302             : 
+     303       31587 :   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       31587 :   return output_yaw_rate;
+     310             : }
+     311             : 
+     312      343853 : double AttitudeConverter::getHeadingRate(const Vector3Converter& attitude_rate) {
+     313             : 
+     314             :   // prep
+     315      343853 :   Eigen::Matrix3d R = *this;
+     316      343829 :   Eigen::Vector3d w = attitude_rate;
+     317             : 
+     318             :   // create the angular velocity tensor
+     319      343755 :   Eigen::Matrix3d W;
+     320      343738 :   W << 0, -w[2], w[1], w[2], 0, -w[0], -w[1], w[0], 0;
+     321             : 
+     322             :   // R derivative
+     323      343750 :   Eigen::Matrix3d R_d = R * W;
+     324             : 
+     325             :   // atan2 derivative
+     326      343819 :   double rx = R(0, 0);  // x-component of body X
+     327      343669 :   double ry = R(1, 0);  // y-component of body Y
+     328             : 
+     329      343742 :   double denom = rx * rx + ry * ry;
+     330             : 
+     331      343742 :   if (fabs(denom) <= 1e-5) {
+     332           0 :     ROS_ERROR("[AttitudeConverter]: getHeadingRate(): denominator near zero!!!");
+     333           0 :     throw MathErrorException();
+     334             :   }
+     335             : 
+     336      343742 :   double atan2_d_x = -ry / denom;
+     337      343742 :   double atan2_d_y = rx / denom;
+     338             : 
+     339             :   // atan2 total differential
+     340      343742 :   double heading_rate = atan2_d_x * R_d(0, 0) + atan2_d_y * R_d(1, 0);
+     341             : 
+     342      343762 :   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      325424 : Vector3Converter AttitudeConverter::getVectorZ(void) {
+     364             : 
+     365      325424 :   tf2::Vector3 b3 = tf2::Vector3(0, 0, 1);
+     366             : 
+     367      325424 :   tf2::Vector3 new_b3 = tf2::Transform(tf2_quaternion_) * b3;
+     368             : 
+     369      650840 :   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      323426 : AttitudeConverter AttitudeConverter::setHeading(const double& heading) {
+     404             : 
+     405             :   // get the Z unit vector after the original rotation
+     406      323426 :   Eigen::Vector3d b3 = getVectorZ();
+     407             : 
+     408             :   // check for singularity: z component of the thrust vector is 0
+     409      323419 :   if (fabs(b3[2]) < 1e-3) {
+     410           5 :     throw SetHeadingException();
+     411             :   }
+     412             : 
+     413             :   // get the desired heading as a vector in 3D
+     414      323419 :   Eigen::Vector3d h(cos(heading), sin(heading), 0);
+     415             : 
+     416      323420 :   Eigen::Matrix3d new_R;
+     417             : 
+     418      323414 :   new_R.col(2) = b3;
+     419             : 
+     420             :   // construct the oblique projection
+     421      323409 :   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      645305 :   Eigen::MatrixXd A = Eigen::MatrixXd(3, 2);
+     425      323182 :   A.col(0)          = projector_body_z_compl.col(0);
+     426      323092 :   A.col(1)          = projector_body_z_compl.col(1);
+     427             : 
+     428             :   // create the basis of the projection null-space complement
+     429      644927 :   Eigen::MatrixXd B = Eigen::MatrixXd(3, 2);
+     430      323156 :   B.col(0)          = Eigen::Vector3d(1, 0, 0);
+     431      323013 :   B.col(1)          = Eigen::Vector3d(0, 1, 0);
+     432             : 
+     433             :   // oblique projector to <range_basis>
+     434      644204 :   Eigen::MatrixXd Bt_A               = B.transpose() * A;
+     435      644892 :   Eigen::MatrixXd Bt_A_pseudoinverse = ((Bt_A.transpose() * Bt_A).inverse()) * Bt_A.transpose();
+     436      322361 :   Eigen::MatrixXd oblique_projector  = A * Bt_A_pseudoinverse * B.transpose();
+     437             : 
+     438      322302 :   new_R.col(0) = oblique_projector * h;
+     439      321781 :   new_R.col(0).normalize();
+     440             : 
+     441             :   // | ------------------------- body y ------------------------- |
+     442             : 
+     443      322011 :   new_R.col(1) = new_R.col(2).cross(new_R.col(0));
+     444      320095 :   new_R.col(1).normalize();
+     445             : 
+     446      643643 :   return AttitudeConverter(new_R);
+     447             : }
+     448             : 
+     449             : //}
+     450             : 
+     451             : // | ------------------------ internal ------------------------ |
+     452             : 
+     453             : /* calculateRPY() //{ */
+     454             : 
+     455      206289 : void AttitudeConverter::calculateRPY(void) {
+     456             : 
+     457      206289 :   if (!got_rpy_) {
+     458             : 
+     459      206289 :     tf2::Matrix3x3(tf2_quaternion_).getRPY(roll_, pitch_, yaw_);
+     460             :   }
+     461      206289 : }
+     462             : 
+     463             : //}
+     464             : 
+     465             : /* validateOrientation() //{ */
+     466             : 
+     467    15005628 : void AttitudeConverter::validateOrientation(void) {
+     468             : 
+     469    30004116 :   if (!std::isfinite(tf2_quaternion_.x()) || !std::isfinite(tf2_quaternion_.y()) || !std::isfinite(tf2_quaternion_.z()) ||
+     470    14994796 :       !std::isfinite(tf2_quaternion_.w())) {
+     471           2 :     throw InvalidAttitudeException();
+     472             :   }
+     473    14996635 : }
+     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..d16e020c88 --- /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-12-17 22:35:29Functions: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..5f252db06c --- /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-12-17 22:35:29Functions: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..91c17b8b0c --- /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-12-17 22:35:29Functions: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..fd112dc25a --- /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-12-17 22:35:29Functions: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..e9926d5032 --- /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-12-17 22:35:29Functions: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..8b28cd8d96 --- /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-12-17 22:35:29Functions: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..495aabe447 --- /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-12-17 22:35:29Functions: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&)85
mrs_lib::BatchVisualizer::initialize()218
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> >)218
mrs_lib::BatchVisualizer::BatchVisualizer()218
mrs_lib::BatchVisualizer::clearBuffers()240
mrs_lib::BatchVisualizer::clearVisuals()240
mrs_lib::BatchVisualizer::~BatchVisualizer()436
mrs_lib::BatchVisualizer::addNullPoint()458
mrs_lib::BatchVisualizer::addNullLine()470
mrs_lib::BatchVisualizer::addNullTriangle()470
mrs_lib::BatchVisualizer::publish()470
+
+
+ + + +
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..1c7ee9c532 --- /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-12-17 22:35:29Functions: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()218
mrs_lib::BatchVisualizer::addCylinder(mrs_lib::geometry::Cylinder const&, double, double, double, double, bool, bool, int, ros::Duration const&)0
mrs_lib::BatchVisualizer::addNullLine()470
mrs_lib::BatchVisualizer::addTriangle(mrs_lib::geometry::Triangle const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::addNullPoint()458
mrs_lib::BatchVisualizer::addRectangle(mrs_lib::geometry::Rectangle const&, double, double, double, double, bool, ros::Duration const&)0
mrs_lib::BatchVisualizer::clearBuffers()240
mrs_lib::BatchVisualizer::clearVisuals()240
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()470
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()470
mrs_lib::BatchVisualizer::addPoint(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double, double, double, double, ros::Duration const&)85
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> >)218
mrs_lib::BatchVisualizer::BatchVisualizer()218
mrs_lib::BatchVisualizer::~BatchVisualizer()436
+
+
+ + + +
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..861abe7786 --- /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-12-17 22:35:29Functions: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         218 : BatchVisualizer::BatchVisualizer() {
+      10         218 : }
+      11             : 
+      12         436 : BatchVisualizer::~BatchVisualizer() {
+      13         436 : }
+      14             : 
+      15         218 : BatchVisualizer::BatchVisualizer(ros::NodeHandle &nh, const std::string marker_topic_name, const std::string parent_frame) {
+      16         218 :   this->parent_frame      = parent_frame;
+      17         218 :   this->marker_topic_name = marker_topic_name;
+      18         218 :   initialize();
+      19             : 
+      20         218 :   this->visual_pub = nh.advertise<visualization_msgs::MarkerArray>(marker_topic_name.c_str(), 1);
+      21         218 :   publish();
+      22         218 : }
+      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         218 : void BatchVisualizer::initialize() {
+      38         218 :   if (initialized) {
+      39           0 :     return;
+      40             :   }
+      41             : 
+      42             :   // setup points marker
+      43         218 :   std::stringstream ss;
+      44         218 :   ss << marker_topic_name << "_points";
+      45         218 :   points_marker.header.frame_id    = parent_frame;
+      46         218 :   points_marker.header.stamp       = ros::Time::now();
+      47         218 :   points_marker.ns                 = ss.str().c_str();
+      48         218 :   points_marker.action             = visualization_msgs::Marker::ADD;
+      49         218 :   points_marker.pose.orientation.w = 1.0;
+      50         218 :   points_marker.id                 = 8;
+      51         218 :   points_marker.type               = visualization_msgs::Marker::POINTS;
+      52             : 
+      53         218 :   points_marker.scale.x = points_scale;
+      54         218 :   points_marker.scale.y = points_scale;
+      55         218 :   points_marker.color.a = 1.0;
+      56             : 
+      57             :   // setup lines marker
+      58         218 :   ss.str(std::string());
+      59         218 :   ss << marker_topic_name << "_lines";
+      60         218 :   lines_marker.header.frame_id    = parent_frame;
+      61         218 :   lines_marker.header.stamp       = ros::Time::now();
+      62         218 :   lines_marker.ns                 = ss.str().c_str();
+      63         218 :   lines_marker.action             = visualization_msgs::Marker::ADD;
+      64         218 :   lines_marker.pose.orientation.w = 1.0;
+      65         218 :   lines_marker.id                 = 5;
+      66         218 :   lines_marker.type               = visualization_msgs::Marker::LINE_LIST;
+      67             : 
+      68         218 :   lines_marker.scale.x = lines_scale;
+      69         218 :   lines_marker.color.a = 1.0;
+      70             : 
+      71             :   // setup triangles marker
+      72         218 :   ss.str(std::string());
+      73         218 :   ss << marker_topic_name << "_triangles";
+      74         218 :   triangles_marker.header.frame_id    = parent_frame;
+      75         218 :   triangles_marker.header.stamp       = ros::Time::now();
+      76         218 :   triangles_marker.ns                 = ss.str().c_str();
+      77         218 :   triangles_marker.action             = visualization_msgs::Marker::ADD;
+      78         218 :   triangles_marker.pose.orientation.w = 1.0;
+      79         218 :   triangles_marker.id                 = 11;
+      80         218 :   triangles_marker.type               = visualization_msgs::Marker::TRIANGLE_LIST;
+      81             : 
+      82         218 :   triangles_marker.scale.x = 1;
+      83         218 :   triangles_marker.scale.y = 1;
+      84         218 :   triangles_marker.scale.z = 1;
+      85         218 :   triangles_marker.color.a = 1.0;
+      86             : 
+      87         218 :   ROS_INFO("[%s]: Batch visualizer loaded with default values", ros::this_node::getName().c_str());
+      88         218 :   initialized = true;
+      89             : }
+      90             : //}
+      91             : 
+      92             : /* addPoint //{ */
+      93          85 : 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         170 :   VisualObject obj = VisualObject(p, r, g, b, a, timeout, uuid++);
+      96          85 :   visual_objects.insert(obj);
+      97          85 : }
+      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         458 : void BatchVisualizer::addNullPoint() {
+     178         458 :   geometry_msgs::Point p;
+     179         458 :   p.x = 10000.0;
+     180         458 :   p.y = 0.0;
+     181         458 :   p.z = 0.0;
+     182             : 
+     183         458 :   std_msgs::ColorRGBA c;
+     184         458 :   c.r = 1.0;
+     185         458 :   c.g = 1.0;
+     186         458 :   c.b = 1.0;
+     187         458 :   c.a = 1.0;
+     188             : 
+     189         458 :   points_marker.points.push_back(p);
+     190         458 :   points_marker.colors.push_back(c);
+     191         458 : }
+     192             : //}
+     193             : 
+     194             : /* addNullLine //{ */
+     195         470 : void BatchVisualizer::addNullLine() {
+     196         470 :   geometry_msgs::Point p1, p2;
+     197         470 :   p1.x = 10000.0;
+     198         470 :   p1.y = 0.0;
+     199         470 :   p1.z = 0.0;
+     200             : 
+     201         470 :   p2.x = 10001.0;
+     202         470 :   p2.y = 0.0;
+     203         470 :   p2.z = 0.0;
+     204             : 
+     205         470 :   std_msgs::ColorRGBA c;
+     206         470 :   c.r = 1.0;
+     207         470 :   c.g = 1.0;
+     208         470 :   c.b = 1.0;
+     209             : 
+     210         470 :   lines_marker.colors.push_back(c);
+     211         470 :   lines_marker.colors.push_back(c);
+     212             : 
+     213         470 :   lines_marker.points.push_back(p1);
+     214         470 :   lines_marker.points.push_back(p2);
+     215         470 : }
+     216             : //}
+     217             : 
+     218             : /* addNullTriangle //{ */
+     219         470 : void BatchVisualizer::addNullTriangle() {
+     220         470 :   geometry_msgs::Point p1, p2, p3;
+     221         470 :   p1.x = 10000.0;
+     222         470 :   p1.y = 0.0;
+     223         470 :   p1.z = 0.0;
+     224             : 
+     225         470 :   p2.x = 10001.0;
+     226         470 :   p2.y = 0.0;
+     227         470 :   p2.z = 0.0;
+     228             : 
+     229         470 :   std_msgs::ColorRGBA c;
+     230         470 :   c.r = 1.0;
+     231         470 :   c.g = 1.0;
+     232         470 :   c.b = 1.0;
+     233             : 
+     234         470 :   p3.x = 10001.0;
+     235         470 :   p3.y = 0.01;
+     236         470 :   p3.z = 0.0;
+     237         470 :   triangles_marker.colors.push_back(c);
+     238         470 :   triangles_marker.colors.push_back(c);
+     239         470 :   triangles_marker.colors.push_back(c);
+     240             : 
+     241         470 :   triangles_marker.points.push_back(p1);
+     242         470 :   triangles_marker.points.push_back(p2);
+     243         470 :   triangles_marker.points.push_back(p3);
+     244         470 : }
+     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         240 : void BatchVisualizer::clearBuffers() {
+     261         240 :   visual_objects.clear();
+     262         240 : }
+     263             : //}
+     264             : 
+     265             : /* clearVisuals //{ */
+     266         240 : void BatchVisualizer::clearVisuals() {
+     267         480 :   std::set<VisualObject> visual_objects_tmp;
+     268         240 :   visual_objects_tmp.insert(visual_objects.begin(), visual_objects.end());
+     269             : 
+     270         240 :   visual_objects.clear();
+     271         240 :   publish();
+     272             : 
+     273         240 :   visual_objects.insert(visual_objects_tmp.begin(), visual_objects_tmp.end());
+     274         240 : }
+     275             : //}
+     276             : 
+     277             : /* publish //{ */
+     278         470 : void BatchVisualizer::publish() {
+     279             : 
+     280         470 :   msg.markers.clear();
+     281         470 :   points_marker.points.clear();
+     282         470 :   points_marker.colors.clear();
+     283             : 
+     284         470 :   lines_marker.points.clear();
+     285         470 :   lines_marker.colors.clear();
+     286             : 
+     287         470 :   triangles_marker.points.clear();
+     288         470 :   triangles_marker.colors.clear();
+     289             : 
+     290             :   // fill marker messages and remove objects that have timed out
+     291         530 :   for (auto it = visual_objects.begin(); it != visual_objects.end();) {
+     292          60 :     if (it->isTimedOut()) {
+     293           0 :       it = visual_objects.erase(it);
+     294             :     } else {
+     295         120 :       auto points = it->getPoints();
+     296         120 :       auto colors = it->getColors();
+     297          60 :       switch (it->getType()) {
+     298          60 :         case MarkerType::POINT: {
+     299          60 :           points_marker.points.insert(points_marker.points.end(), points.begin(), points.end());
+     300          60 :           points_marker.colors.insert(points_marker.colors.end(), colors.begin(), colors.end());
+     301          60 :           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          60 :       it++;
+     315             :     }
+     316             :   }
+     317             : 
+     318         470 :   auto now = ros::Time::now();
+     319             : 
+     320         470 :   if (!points_marker.points.empty()) {
+     321          12 :     points_marker.scale.x = points_scale;
+     322          12 :     points_marker.scale.y = points_scale;
+     323             :   } else {
+     324         458 :     addNullPoint();
+     325             :   }
+     326         470 :   points_marker.header.stamp = now;
+     327         470 :   msg.markers.push_back(points_marker);
+     328             : 
+     329         470 :   if (!lines_marker.points.empty()) {
+     330           0 :     lines_marker.scale.x = lines_scale;
+     331             :   } else {
+     332         470 :     addNullLine();
+     333             :   }
+     334         470 :   lines_marker.header.stamp = now;
+     335         470 :   msg.markers.push_back(lines_marker);
+     336             : 
+     337         470 :   if (!triangles_marker.points.empty()) {
+     338           0 :     triangles_marker.header.stamp = now;
+     339             :   } else {
+     340         470 :     addNullTriangle();
+     341             :   }
+     342         470 :   triangles_marker.header.stamp = now;
+     343         470 :   msg.markers.push_back(triangles_marker);
+     344             : 
+     345         470 :   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         470 :   visual_pub.publish(msg);
+     353         470 : }
+     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-12-17 22:35:29Functions: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..60513a40d0 --- /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-12-17 22:35:29Functions: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..6dd7f795a6 --- /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-12-17 22:35:29Functions: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..edf75c02bc --- /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-12-17 22:35:29Functions: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..872f2bea0b --- /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-12-17 22:35:29Functions: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..5a3ecdcfad --- /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-12-17 22:35:29Functions: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..4d10575a47 --- /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-12-17 22:35:29Functions: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() const60
mrs_lib::VisualObject::getType() const60
mrs_lib::VisualObject::getColors() const60
mrs_lib::VisualObject::getPoints() const60
mrs_lib::eigenToMsg(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)85
mrs_lib::VisualObject::VisualObject(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double, double, double, double, ros::Duration const&, unsigned long const&)85
mrs_lib::generateColor(double, double, double, double)85
+
+
+ + + +
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..6f5bb5b8f8 --- /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-12-17 22:35:29Functions: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&)85
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&)85
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)85
mrs_lib::VisualObject::isTimedOut() const60
mrs_lib::VisualObject::getID() const0
mrs_lib::VisualObject::getType() const60
mrs_lib::VisualObject::getColors() const60
mrs_lib::VisualObject::getPoints() const60
+
+
+ + + +
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..2688e7e523 --- /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-12-17 22:35:29Functions: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          85 : geometry_msgs::Point eigenToMsg(const Eigen::Vector3d& v) {
+      10          85 :   geometry_msgs::Point p;
+      11          85 :   p.x = v.x();
+      12          85 :   p.y = v.y();
+      13          85 :   p.z = v.z();
+      14          85 :   return p;
+      15             : }
+      16             : 
+      17          85 : std_msgs::ColorRGBA generateColor(const double r, const double g, const double b, const double a) {
+      18          85 :   std_msgs::ColorRGBA c;
+      19          85 :   c.r = r;
+      20          85 :   c.g = g;
+      21          85 :   c.b = b;
+      22          85 :   c.a = a;
+      23          85 :   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          85 : VisualObject::VisualObject(const Eigen::Vector3d& point, const double r, const double g, const double b, const double a, const ros::Duration& timeout,
+     116          85 :                            const unsigned long& id)
+     117          85 :     : id_(id) {
+     118          85 :   type_ = MarkerType::POINT;
+     119          85 :   points_.push_back(eigenToMsg(point));
+     120          85 :   colors_.push_back(generateColor(r, g, b, a));
+     121          85 :   if (timeout.toSec() <= 0) {
+     122          85 :     timeout_time_ = ros::Time(0);
+     123             :   } else {
+     124           0 :     timeout_time_ = ros::Time::now() + timeout;
+     125             :   }
+     126          85 : }
+     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          60 : int VisualObject::getType() const {
+     329          60 :   return type_;
+     330             : }
+     331             : //}
+     332             : 
+     333             : /* isTimedOut //{ */
+     334          60 : bool VisualObject::isTimedOut() const {
+     335          60 :   return !timeout_time_.isZero() && (timeout_time_ - ros::Time::now()).toSec() <= 0;
+     336             : }
+     337             : //}
+     338             : 
+     339             : /* getPoints //{ */
+     340          60 : const std::vector<geometry_msgs::Point> VisualObject::getPoints() const {
+     341          60 :   return points_;
+     342             : }
+     343             : //}
+     344             : 
+     345             : /* getColors //{ */
+     346          60 : const std::vector<std_msgs::ColorRGBA> VisualObject::getColors() const {
+     347          60 :   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-12-17 22:35:29Functions: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&)153275
mrs_lib::geometry::fromEigen(Eigen::Quaternion<double, 0> const&)153275
+
+
+ + + +
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..7a0102f5de --- /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-12-17 22:35:29Functions: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&)153275
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&)153275
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..0d37aac622 --- /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-12-17 22:35:29Functions: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      153275 :     geometry_msgs::Quaternion fromEigen(const Eigen::Quaterniond& what)
+      48             :     {
+      49      153275 :       geometry_msgs::Quaternion q;
+      50      153275 :       q.x = what.x();
+      51      153275 :       q.y = what.y();
+      52      153275 :       q.z = what.z();
+      53      153275 :       q.w = what.w();
+      54      153275 :       return q;
+      55             :     }
+      56             :     
+      57      153275 :     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      153275 :       Eigen::Quaterniond q;
+      61      153275 :       q.x() = what.x;
+      62      153275 :       q.y() = what.y;
+      63      153275 :       q.z() = what.z;
+      64      153275 :       q.w() = what.w;
+      65      153275 :       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..01cef5b572 --- /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-12-17 22:35:29Functions: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..591e46de23 --- /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-12-17 22:35:29Functions: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..7648d01580 --- /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-12-17 22:35:29Functions: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..2a0a51b843 --- /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-12-17 22:35:29Functions: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..19757b33a2 --- /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-12-17 22:35:29Functions: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..ba75e87b57 --- /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-12-17 22:35:29Functions: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..94138608d4 --- /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-12-17 22:35:29Functions: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::angleBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)140120
mrs_lib::geometry::quaternionFromHeading(double)153281
mrs_lib::geometry::dist(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)233142
mrs_lib::geometry::dist(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)375360
+
+
+ + + +
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..094264e174 --- /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-12-17 22:35:29Functions: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&)140120
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)153281
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&)375360
mrs_lib::geometry::dist(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)233142
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..767b55f1f2 --- /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-12-17 22:35:29Functions: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      140120 :     double angleBetween(const vec3_t& vec1, const vec3_t& vec2)
+      29             :     {
+      30      140120 :       const double sin_12 = vec1.cross(vec2).norm();
+      31      140109 :       const double cos_12 = vec1.dot(vec2);
+      32      140099 :       const double angle = std::atan2(sin_12, cos_12);
+      33      140099 :       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      153281 :     quat_t quaternionFromHeading(const double heading)
+      93             :     {
+      94      306562 :       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      375360 :     double dist(const vec2_t& a, const vec2_t& b)
+     171             :     {
+     172             : 
+     173      375360 :       return (a - b).norm();
+     174             :     }
+     175             : 
+     176      233142 :     double dist(const vec3_t& a, const vec3_t& b)
+     177             :     {
+     178             : 
+     179      233142 :       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-12-17 22:35:29Functions: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..c2285b0337 --- /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-12-17 22:35:29Functions: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..a61d57aeea --- /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-12-17 22:35:29Functions: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-12-17 22:35:29Functions: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..7093fa7574 --- /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-12-17 22:35:29Functions: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..edaab314d4 --- /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-12-17 22:35:29Functions: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..a21ee7ddf0 --- /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-12-17 22:35:29Functions: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..43939fbaf6 --- /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-12-17 22:35:29Functions: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..5d8039391b --- /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-12-17 22:35:29Functions: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..d540a91c2f --- /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-12-17 22:35:29Functions: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..9a573e72c0 --- /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-12-17 22:35:29Functions: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..f57a821be6 --- /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-12-17 22:35:29Functions: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-12-17 22:35:29Functions: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..89550309b8 --- /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-12-17 22:35:29Functions: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..e725170c0b --- /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-12-17 22:35:29Functions: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..eda7ef7345 --- /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-12-17 22:35:29Functions: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..64d4ac7920 --- /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-12-17 22:35:29Functions: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..949927877c --- /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-12-17 22:35:29Functions: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..80aecb45b5 --- /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-12-17 22:35:29Functions: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&&)85
mrs_lib::MedianFilter::operator=(mrs_lib::MedianFilter const&)86
mrs_lib::MedianFilter::MedianFilter(unsigned long, double, double, double)88
mrs_lib::MedianFilter::check(double)131814
mrs_lib::MedianFilter::median() const131831
mrs_lib::MedianFilter::full() const140121
mrs_lib::MedianFilter::add(double)140141
+
+
+ + + +
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..9493423eb1 --- /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-12-17 22:35:29Functions: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)140141
mrs_lib::MedianFilter::check(double)131814
mrs_lib::MedianFilter::clear()2
mrs_lib::MedianFilter::addCheck(double)26
mrs_lib::MedianFilter::MedianFilter(mrs_lib::MedianFilter&&)85
mrs_lib::MedianFilter::MedianFilter(mrs_lib::MedianFilter const&)1
mrs_lib::MedianFilter::MedianFilter(unsigned long, double, double, double)88
mrs_lib::MedianFilter::MedianFilter()1
mrs_lib::MedianFilter::operator=(mrs_lib::MedianFilter&&)0
mrs_lib::MedianFilter::operator=(mrs_lib::MedianFilter const&)86
mrs_lib::MedianFilter::initialized() const1
mrs_lib::MedianFilter::full() const140121
mrs_lib::MedianFilter::median() const131831
+
+
+ + + +
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..de9afc25c9 --- /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-12-17 22:35:29Functions: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          88 :   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          88 :       m_max_diff(max_diff)
+      12             :   {
+      13          88 :     m_buffer.set_capacity(buffer_length);
+      14          88 :     m_buffer_sorted.reserve(buffer_length);
+      15          88 :   }
+      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          85 :   MedianFilter::MedianFilter(MedianFilter&& other)
+      32             :   {
+      33          85 :     *this = other;
+      34          85 :   }
+      35             : 
+      36             :   //}
+      37             : 
+      38             :   /* operator=() method and overloads //{ */
+      39          86 :   MedianFilter& MedianFilter::operator=(const MedianFilter& other)
+      40             :   {
+      41          86 :     std::scoped_lock lck(other.m_mtx, m_mtx);
+      42             :   
+      43          86 :     m_buffer = other.m_buffer;
+      44          86 :     m_buffer_sorted = other.m_buffer_sorted;
+      45          86 :     m_median = other.m_median;
+      46             :   
+      47             :     // parameters specified by the user
+      48          86 :     m_min_valid = other.m_min_valid;
+      49          86 :     m_max_valid = other.m_max_valid;
+      50          86 :     m_max_diff = other.m_max_diff;
+      51             :   
+      52         172 :     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      140141 :   void MedianFilter::add(const double value)
+      74             :   {
+      75      280242 :     std::scoped_lock lck(m_mtx);
+      76             :     // add the value to the buffer
+      77      140136 :     m_buffer.push_back(value);
+      78             :     // reset the cached median value
+      79      140126 :     m_median = std::nullopt;
+      80      140121 :   }
+      81             :   //}
+      82             : 
+      83             :   /* check() method //{ */
+      84      131814 :   bool MedianFilter::check(const double value)
+      85             :   {
+      86      131814 :     std::scoped_lock lck(m_mtx);
+      87             :     // check if all constraints are met
+      88      131811 :     const double diff = m_buffer.empty() ? 0.0 : std::abs(median() - value);
+      89      263625 :     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      140121 :   bool MedianFilter::full() const
+     113             :   {
+     114      280242 :     std::scoped_lock lck(m_mtx);
+     115      280242 :     return m_buffer.full();
+     116             :   }
+     117             :   //}
+     118             : 
+     119             :   /* median() method //{ */
+     120      131831 :   double MedianFilter::median() const
+     121             :   {
+     122      263671 :     std::scoped_lock lck(m_mtx);
+     123             :     // if the value was already calculated, just return it
+     124      131825 :     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      131798 :     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      131794 :     m_buffer_sorted.clear();
+     136             :     // copy all elements from the input buffer to buffer_sorted
+     137      131807 :     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      131809 :     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      131804 :     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      131800 :     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      131819 :     if (even_set)
+     148      131803 :       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      131807 :     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-12-17 22:35:29Functions: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..3a40b4ee5b --- /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-12-17 22:35:29Functions: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..7609f175ec --- /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-12-17 22:35:29Functions: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..8a9b214803 --- /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-12-17 22:35:29Functions: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..a89abc3ae3 --- /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-12-17 22:35:29Functions: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..2660456332 --- /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-12-17 22:35:29Functions: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..7b07b9bb7a --- /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-12-17 22:35:29Functions: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)4741
mrs_lib::ParamProvider::addYamlFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)15677
mrs_lib::ParamProvider::findYamlNode(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) const104887
+
+
+ + + +
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..83b8df3fba --- /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-12-17 22:35:29Functions: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&)15677
mrs_lib::ParamProvider::ParamProvider(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool)4741
mrs_lib::ParamProvider::findYamlNode(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) const104887
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..ac049d9b20 --- /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-12-17 22:35:29Functions: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        4741 :   ParamProvider::ParamProvider(const ros::NodeHandle& nh, std::string node_name, const bool use_rosparam)
+      14        4741 :   : m_nh(nh), m_node_name(std::move(node_name)), m_use_rosparam(use_rosparam)
+      15             :   {
+      16        4741 :   }
+      17             : 
+      18       15677 :   bool ParamProvider::addYamlFile(const std::string& filepath)
+      19             :   {
+      20             :     try
+      21             :     {
+      22       31354 :       const auto loaded_yaml = YAML::LoadFile(filepath);
+      23       15677 :       YAML::Node root;
+      24       15677 :       root["root"] = loaded_yaml;
+      25       15677 :       m_yamls.emplace_back(root);
+      26       15677 :       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      104887 :   std::optional<YAML::Node> ParamProvider::findYamlNode(const std::string& param_name) const
+      65             :   {
+      66      547814 :     for (const auto& yaml : m_yamls)
+      67             :     {
+      68             :       // Try to load the parameter sequentially as a map.
+      69      528667 :       auto cur_node_it = std::cbegin(yaml);
+      70             :       // The root should always be a pam
+      71      528669 :       if (!cur_node_it->second.IsMap())
+      72       16951 :         continue;
+      73             : 
+      74      511719 :       bool loaded = true;
+      75             :       {
+      76      511719 :         constexpr char delimiter = '/';
+      77      511719 :         auto substr_start = std::cbegin(param_name);
+      78      511720 :         auto substr_end = substr_start;
+      79      694179 :         do
+      80             :         {
+      81     1205899 :           substr_end = std::find(substr_start, std::cend(param_name), delimiter);
+      82             :           // why can't substr or string_view take iterators? :'(
+      83     1205889 :           const auto start_pos = std::distance(std::cbegin(param_name), substr_start);
+      84     1205885 :           const auto count = std::distance(substr_start, substr_end);
+      85     1205872 :           const std::string param_substr = param_name.substr(start_pos, count);
+      86     1205874 :           substr_start = substr_end+1;
+      87             : 
+      88     1205886 :           bool found = false;
+      89     4929756 :           for (auto node_it = std::cbegin(cur_node_it->second); node_it != std::cend(cur_node_it->second); ++node_it)
+      90             :           {
+      91     2517961 :             if (node_it->first.as<std::string>() == param_substr)
+      92             :             {
+      93      779917 :               cur_node_it = node_it;
+      94      779922 :               found = true;
+      95      779922 :               break;
+      96             :             }
+      97             :           }
+      98             : 
+      99     1205900 :           if (!found)
+     100             :           {
+     101      425978 :             loaded = false;
+     102      425978 :             break;
+     103             :           }
+     104             :         }
+     105      779918 :         while (substr_end != std::end(param_name) && cur_node_it->second.IsMap());
+     106             :       }
+     107             : 
+     108      511719 :       if (loaded)
+     109             :       {
+     110      171487 :         return cur_node_it->second;
+     111             :       }
+     112             :     }
+     113             : 
+     114       19143 :     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:3710136.6 %
Date:2024-12-17 22:35:29Functions: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 +
36.6%36.6%
+ 		36.6 %37 / 10190.0 %9 / 10
<unnamed>36.6 %37 / 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..da4d1829e8 --- /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:3710136.6 %
Date:2024-12-17 22:35:29Functions: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 +
36.6%36.6%
+ 		36.6 %37 / 10190.0 %9 / 10
<unnamed>36.6 %37 / 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..decbf172f8 --- /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:3710136.6 %
Date:2024-12-17 22:35:29Functions: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 +
36.6%36.6%
+ 		36.6 %37 / 10190.0 %9 / 10
<unnamed>36.6 %37 / 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..59e6b27a32 --- /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:3710136.6 %
Date:2024-12-17 22:35:29Functions: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 +
36.6%36.6%
+ 		36.6 %37 / 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..9ed9d56953 --- /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:3710136.6 %
Date:2024-12-17 22:35:29Functions: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 +
36.6%36.6%
+ 		36.6 %37 / 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..f8950c768a --- /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:3710136.6 %
Date:2024-12-17 22:35:29Functions: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 +
36.6%36.6%
+ 		36.6 %37 / 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..d451d21f3e --- /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:3710136.6 %
Date:2024-12-17 22:35:29Functions: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)1527
mrs_lib::Profiler::Profiler()1527
mrs_lib::Profiler::operator=(mrs_lib::Profiler const&)1527
mrs_lib::Profiler::createRoutine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, double, ros::TimerEvent)755104
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)755197
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)2116645
mrs_lib::Profiler::createRoutine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)2116680
mrs_lib::Routine::end()2871615
mrs_lib::Routine::~Routine()2871844
+
+
+ + + +
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..35e7b68a8b --- /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:3710136.6 %
Date:2024-12-17 22:35:29Functions:91090.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Routine::end()2871615
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)2116645
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)755197
mrs_lib::Routine::~Routine()2871844
mrs_lib::Profiler::createRoutine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)2116680
mrs_lib::Profiler::createRoutine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, double, ros::TimerEvent)755104
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)1527
mrs_lib::Profiler::Profiler()1527
mrs_lib::Profiler::operator=(mrs_lib::Profiler const&)1527
+
+
+ + + +
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..3eedfa457e --- /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:3710136.6 %
Date:2024-12-17 22:35:29Functions: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        1527 : Profiler::Profiler() {
+      11        1527 : }
+      12             : 
+      13        1527 : Profiler::Profiler(ros::NodeHandle& nh, std::string _node_name_, bool profiler_enabled) {
+      14             : 
+      15        1527 :   this->nh_                = std::make_shared<ros::NodeHandle>(nh);
+      16        1527 :   this->_node_name_        = _node_name_;
+      17        1527 :   this->_profiler_enabled_ = profiler_enabled;
+      18             : 
+      19        1527 :   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        1527 :   ROS_INFO("[%s]: profiler initialized", _node_name_.c_str());
+      25             : 
+      26        1527 :   this->is_initialized_ = true;
+      27        1527 : }
+      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        1527 : Profiler& Profiler::operator=(const Profiler& other) {
+      43             : 
+      44        1527 :   if (this == &other) {
+      45           0 :     return *this;
+      46             :   }
+      47             : 
+      48        1527 :   this->is_initialized_    = other.is_initialized_;
+      49        1527 :   this->nh_                = other.nh_;
+      50        1527 :   this->_node_name_        = other._node_name_;
+      51        1527 :   this->_profiler_enabled_ = other._profiler_enabled_;
+      52             : 
+      53        1527 :   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        1527 :   return *this;
+      59             : }
+      60             : 
+      61             : //}
+      62             : 
+      63             : /* Profiler::registerRoutine() for periodic //{ */
+      64             : 
+      65      755104 : Routine Profiler::createRoutine(std::string name, double expected_rate, double threshold, ros::TimerEvent event) {
+      66             : 
+      67      755104 :   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     2116680 : Routine Profiler::createRoutine(std::string name) {
+      75             : 
+      76     2116680 :   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      755197 : Routine::Routine(std::string name, std::string node_name, double expected_rate, double threshold, std::shared_ptr<ros::Publisher> publisher,
+      86      755197 :                  std::shared_ptr<std::mutex> mutex_publisher, bool profiler_enabled, ros::TimerEvent event) {
+      87             : 
+      88      754249 :   if (!profiler_enabled) {
+      89      754304 :     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     2116645 : Routine::Routine(std::string name, std::string node_name, std::shared_ptr<ros::Publisher> publisher, std::shared_ptr<std::mutex> mutex_publisher,
+     141     2116645 :                  bool profiler_enabled) {
+     142             : 
+     143     2115772 :   if (!profiler_enabled) {
+     144     2115800 :     return;
+     145             :   }
+     146             : 
+     147           0 :   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     2871615 : void Routine::end(void) {
+     186             : 
+     187     2871615 :   if (!_profiler_enabled_) {
+     188     2871343 :     return;
+     189             :   }
+     190             : 
+     191         271 :   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     2871493 : Routine::~Routine() {
+     213             : 
+     214     2871844 :   this->end();
+     215     2869917 : }
+     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..44bb60b2c679000bfc9fc7b4f89cc2f0264fcdd8 GIT binary patch literal 827 zcmV-B1H}A^P)( zRA_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^&jikY4nYD1YiIG{gVwg(KnwCXoPcj7rxDq(pec_y~%0UE`vn zQliCteU$ruK}vT0f>axsRe*Bg-SD|$`**`bKdAnZox literal 0 HcmV?d00001 diff --git a/mrs_lib/src/safety_zone/index-detail-sort-f.html b/mrs_lib/src/safety_zone/index-detail-sort-f.html new file mode 100644 index 0000000000..237d5796bb --- /dev/null +++ b/mrs_lib/src/safety_zone/index-detail-sort-f.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-12-17 22:35:29Functions: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..8d68df5db2 --- /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-12-17 22:35:29Functions: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..b37280eaaa --- /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-12-17 22:35:29Functions: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..ad5f30b67a --- /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-12-17 22:35:29Functions: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..5063829b5f --- /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-12-17 22:35:29Functions: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..6c08b05df6 --- /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-12-17 22:35:29Functions: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..7d5600cabb --- /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-12-17 22:35:29Functions: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>)530
mrs_lib::Intersection::Intersection(bool, bool, Eigen::Matrix<double, 1, 2, 1, 1, 2>)568
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>)568
+
+
+ + + +
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..9f4c7d3a91 --- /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-12-17 22:35:29Functions: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>)530
mrs_lib::Intersection::Intersection(bool, bool, Eigen::Matrix<double, 1, 2, 1, 1, 2>)568
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>)568
+
+
+ + + +
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..f8b90fd81e --- /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-12-17 22:35:29Functions: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         530 : 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         530 :   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         568 : Intersection::Intersection(bool intersect, bool parallel, Eigen::RowVector2d point) : point(std::move(point)), parallel(parallel), intersect(intersect){}
+      16             : 
+      17             : /* sectionIntersect() //{ */
+      18             : 
+      19         568 : Intersection sectionIntersect(Eigen::RowVector2d start1, Eigen::RowVector2d end1, Eigen::RowVector2d start2, Eigen::RowVector2d end2) {
+      20         568 :   Eigen::RowVector2d vector1 = end1 - start1;
+      21         568 :   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         568 :   double cross            = vector1(0) * vector2(1) - vector1(1) * vector2(0);
+      29         568 :   double difference_cross = (start2(0) - start1(0)) * vector2(1) - (start2(1) - start1(1)) * vector2(0);
+      30             : 
+      31         568 :   if (cross == 0) {
+      32             :     // are parallel
+      33          38 :     if (difference_cross != 0)
+      34          38 :       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         530 :   double             scale1 = difference_cross / cross;
+      48         530 :   Eigen::RowVector2d point  = start1 + scale1 * vector1;
+      49         530 :   double             scale2 = getScale(start2, vector2, point);
+      50             : 
+      51         530 :   if (0 <= scale1 && scale1 <= 1 && 0 <= scale2 && scale2 <= 1) {
+      52           0 :     return Intersection{true, false, point};
+      53             :   }
+      54         530 :   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..6cd3350f2d --- /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-12-17 22:35:29Functions: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..3637117c58 --- /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-12-17 22:35:29Functions: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..e989c73f41 --- /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-12-17 22:35:29Functions: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..fd603c55fd --- /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-12-17 22:35:29Functions: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..714685763e --- /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-12-17 22:35:29Functions: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..c3672a84df --- /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-12-17 22:35:29Functions: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..4ac571c9ec --- /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-12-17 22:35:29Functions: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>)87
mrs_lib::Polygon::doesSectionIntersect(double, double, double, double)142
mrs_lib::Polygon::isPointInside(double, double)10700
mrs_lib::Polygon::getPointMessageVector(double)25608
+
+
+ + + +
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..562907adb7 --- /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-12-17 22:35:29Functions: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)10700
mrs_lib::Polygon::doesSectionIntersect(double, double, double, double)142
mrs_lib::Polygon::getPointMessageVector(double)25608
mrs_lib::Polygon::Polygon(Eigen::Matrix<double, -1, -1, 0, -1, -1>)87
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..a028fb9aa1 --- /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-12-17 22:35:29Functions: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          87 : Polygon::Polygon(const Eigen::MatrixXd vertices) : vertices(vertices) {
+      10             : 
+      11          87 :   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          87 :   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          87 :   Eigen::RowVector2d edge1;
+      21          87 :   Eigen::RowVector2d edge2 = vertices.row(1) - vertices.row(0);
+      22         435 :   for (int i = 0; i < vertices.rows(); ++i) {
+      23         348 :     edge1 = edge2;
+      24         348 :     edge2 = vertices.row((i + 2) % vertices.rows()) - vertices.row((i + 1) % vertices.rows());
+      25             : 
+      26         348 :     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         348 :     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          87 : }
+      36             : 
+      37             : //}
+      38             : 
+      39             : /* isPointInside() //{ */
+      40             : 
+      41       10700 : bool Polygon::isPointInside(const double px, const double py) {
+      42             : 
+      43       10700 :   int count = 0;
+      44             : 
+      45             :   // Cast a horizontal ray and see how many times it intersects all the edges
+      46       53500 :   for (int i = 0; i < vertices.rows(); ++i) {
+      47       42800 :     double v1x = vertices(i, 0);
+      48       42800 :     double v1y = vertices(i, 1);
+      49       42800 :     double v2x = vertices((i + 1) % vertices.rows(), 0);
+      50       42800 :     double v2y = vertices((i + 1) % vertices.rows(), 1);
+      51             : 
+      52       42800 :     if (v1y > py && v2y > py)
+      53       10624 :       continue;
+      54       32176 :     if (v1y <= py && v2y <= py)
+      55       10928 :       continue;
+      56       21248 :     double intersect_x    = (v1y == v2y) ? v1x : (py - v1y) * (v2x - v1x) / (v2y - v1y) + v1x;
+      57       21248 :     bool   does_intersect = intersect_x > px;
+      58       21248 :     if (does_intersect)
+      59       10621 :       ++count;
+      60             :   }
+      61             : 
+      62       10700 :   return count % 2;
+      63             : }
+      64             : 
+      65             : //}
+      66             : 
+      67             : /* doesSectionIntersect() //{ */
+      68             : 
+      69         142 : bool Polygon::doesSectionIntersect(const double startX, const double startY, const double endX, const double endY) {
+      70             : 
+      71         142 :   Eigen::RowVector2d start{startX, startY};
+      72         142 :   Eigen::RowVector2d end{endX, endY};
+      73             : 
+      74         710 :   for (int i = 0; i < vertices.rows(); ++i) {
+      75             : 
+      76         568 :     Eigen::RowVector2d edgeStart = vertices.row(i);
+      77         568 :     Eigen::RowVector2d edgeEnd   = vertices.row((i + 1) % vertices.rows());
+      78             : 
+      79         568 :     if (sectionIntersect(start, end, edgeStart, edgeEnd).intersect) {
+      80           0 :       return true;
+      81             :     }
+      82             :   }
+      83             : 
+      84         142 :   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       25608 : std::vector<geometry_msgs::Point> Polygon::getPointMessageVector(const double z) {
+     194       25608 :   std::vector<geometry_msgs::Point> points(vertices.rows());
+     195      128040 :   for (int i = 0; i < vertices.rows(); ++i) {
+     196      102432 :     points[i].x = vertices(i, 0);
+     197      102432 :     points[i].y = vertices(i, 1);
+     198      102432 :     points[i].z = z;
+     199             :   }
+     200             : 
+     201       25608 :   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..56ec24da8c --- /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-12-17 22:35:29Functions: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&)87
mrs_lib::SafetyZone::~SafetyZone()87
mrs_lib::SafetyZone::isPathValid(double, double, double, double)142
mrs_lib::SafetyZone::isPointValid(double, double)10700
mrs_lib::SafetyZone::getBorder()12804
+
+
+ + + +
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..c97438e5ab --- /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-12-17 22:35:29Functions: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)142
mrs_lib::SafetyZone::isPointValid(double, double)10700
mrs_lib::SafetyZone::getBorder()12804
mrs_lib::SafetyZone::SafetyZone(mrs_lib::Polygon)0
mrs_lib::SafetyZone::SafetyZone(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&)87
mrs_lib::SafetyZone::~SafetyZone()87
+
+
+ + + +
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..346c864dec --- /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-12-17 22:35:29Functions: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         174 : SafetyZone::~SafetyZone() {
+      17          87 :   delete outerBorder;
+      18          87 : }
+      19             : 
+      20             : //}
+      21             : 
+      22             : /* SafetyZone() //{ */
+      23             : 
+      24          87 : SafetyZone::SafetyZone(const Eigen::MatrixXd& outerBorderMatrix) {
+      25             : 
+      26             :   try {
+      27          87 :     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          87 : }
+      39             : 
+      40             : //}
+      41             : 
+      42             : /* isPointValid() //{ */
+      43             : 
+      44       10700 : bool SafetyZone::isPointValid(const double px, const double py) {
+      45             : 
+      46       10700 :   if (!outerBorder->isPointInside(px, py)) {
+      47          79 :     return false;
+      48             :   }
+      49             : 
+      50       10621 :   return true;
+      51             : }
+      52             : 
+      53             : //}
+      54             : 
+      55             : /* isPathValid() //{ */
+      56             : 
+      57         142 : bool SafetyZone::isPathValid(const double p1x, const double p1y, const double p2x, const double p2y) {
+      58             : 
+      59         142 :   if (outerBorder->doesSectionIntersect(p1x, p1y, p2x, p2y)) {
+      60           0 :     return false;
+      61             :   }
+      62             : 
+      63         142 :   return true;
+      64             : }
+      65             : 
+      66             : //}
+      67             : 
+      68             : /* getBorder() //{ */
+      69             : 
+      70       12804 : Polygon SafetyZone::getBorder() {
+      71       12804 :   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-12-17 22:35:29Functions: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..0a29b4da91 --- /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-12-17 22:35:29Functions: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..633af16e74 --- /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-12-17 22:35:29Functions: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..1262ca5543 --- /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-12-17 22:35:29Functions: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..7b18d97a85 --- /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-12-17 22:35:29Functions: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..d5c0de586f --- /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-12-17 22:35:29Functions: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..ea399689ed --- /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-12-17 22:35:29Functions: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)763
mrs_lib::ScopeTimerLogger::~ScopeTimerLogger()763
mrs_lib::ScopeTimer::checkpoint(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)4648226
mrs_lib::ScopeTimer::ScopeTimer(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_lib::ScopeTimerLogger>, bool)5819748
mrs_lib::ScopeTimer::~ScopeTimer()5821898
+
+
+ + + +
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..296f61b5c9 --- /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-12-17 22:35:29Functions: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&)4648226
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)5819748
mrs_lib::ScopeTimer::~ScopeTimer()5821898
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)763
mrs_lib::ScopeTimerLogger::~ScopeTimerLogger()763
+
+
+ + + +
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..7cb0fc45fd --- /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-12-17 22:35:29Functions: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         763 : ScopeTimerLogger::ScopeTimerLogger(const std::string& logfile, const bool enable_logging, const int log_precision)
+      10         763 :     : _logging_enabled_(enable_logging), _log_filepath_(logfile) {
+      11             : 
+      12         763 :   if (!_logging_enabled_) {
+      13         763 :     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         763 : ScopeTimerLogger::~ScopeTimerLogger() {
+      47         763 :   if (_logging_enabled_) {
+      48           0 :     ROS_DEBUG("[%s]: ScopeTimerLogger: closing logstream.", ros::this_node::getName().c_str());
+      49           0 :     _logstream_.close();
+      50             :   }
+      51         763 : }
+      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     5819748 : ScopeTimer::ScopeTimer(const std::string& label, const std::shared_ptr<ScopeTimerLogger> scope_timer_logger, const bool enable)
+     104     5819748 :     : _timer_label_(label), _enable_print_or_log(enable), _throttle_period_(ros::Duration(0.0)), _logger_(scope_timer_logger) {
+     105             : 
+     106     5806992 :   checkpoints.push_back(time_point("timer start"));
+     107             : 
+     108     5806029 :   ROS_DEBUG("[%s] Scope timer started with file logger (label: %s).", ros::this_node::getName().c_str(), label.c_str());
+     109     5806031 : }
+     110             : 
+     111             : //}
+     112             : 
+     113             : /* ScopeTimer::checkpoint() //{ */
+     114             : 
+     115     4648226 : void ScopeTimer::checkpoint(const std::string& label) {
+     116             : 
+     117     4648226 :   if (_enable_print_or_log) {
+     118           0 :     checkpoints.push_back(time_point(label));
+     119             :   }
+     120     4648226 : }
+     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    17460412 : ScopeTimer::~ScopeTimer() {
+     136             : 
+     137     5821898 :   if (!_enable_print_or_log) {
+     138     5821957 :     return;
+     139             :   }
+     140             : 
+     141          18 :   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     5819507 : }
+     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-12-17 22:35:29Functions: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..2aea9c8403 --- /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-12-17 22:35:29Functions: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..36e66a7372 --- /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-12-17 22:35:29Functions: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..3b05dfc97b --- /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-12-17 22:35:29Functions: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..ecdc7d70f5 --- /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-12-17 22:35:29Functions: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..05476bbf60 --- /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-12-17 22:35:29Functions: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..ddf89d3746 --- /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-12-17 22:35:29Functions: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&)96
mrs_lib::TimeoutManager::registerNew(ros::Duration const&, std::function<void (ros::Time const&)> const&, ros::Time const&, bool, bool)99
mrs_lib::TimeoutManager::start(unsigned long, ros::Time const&)99
mrs_lib::TimeoutManager::started(unsigned long)1402
mrs_lib::TimeoutManager::main_timer_callback(ros::TimerEvent const&)20854
mrs_lib::TimeoutManager::reset(unsigned long, ros::Time const&)182221
+
+
+ + + +
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..3bc528391b --- /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-12-17 22:35:29Functions: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)99
mrs_lib::TimeoutManager::main_timer_callback(ros::TimerEvent const&)20854
mrs_lib::TimeoutManager::pause(unsigned long)9
mrs_lib::TimeoutManager::reset(unsigned long, ros::Time const&)182221
mrs_lib::TimeoutManager::start(unsigned long, ros::Time const&)99
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)1402
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&)96
+
+
+ + + +
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..6b31c0ed7b --- /dev/null +++ b/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.html @@ -0,0 +1,193 @@ + + + + + + + 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-12-17 22:35:29Functions: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          96 :   TimeoutManager::TimeoutManager(const ros::NodeHandle& nh, const ros::Rate& update_rate)
+       7          96 :     : m_last_id(0)
+       8             :   {
+       9          96 :     m_main_timer = nh.createTimer(update_rate, &TimeoutManager::main_timer_callback, this);
+      10          96 :   }
+      11             : 
+      12             : 
+      13          99 :   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          99 :     std::scoped_lock lck(m_mtx);
+      16          99 :     const auto new_id = m_timeouts.size();
+      17             :     m_timeouts.emplace_back(
+      18          99 :       timeout_info_t{
+      19             :         oneshot,
+      20             :         autostart,
+      21             :         callback,
+      22             :         timeout,
+      23             :         last_reset,
+      24             :         last_reset
+      25          99 :       });
+      26         198 :     return new_id;
+      27             :   }
+      28             : 
+      29      182221 :   void TimeoutManager::reset(const timeout_id_t id, const ros::Time& time)
+      30             :   {
+      31      182221 :     std::scoped_lock lck(m_mtx);
+      32      182222 :     m_timeouts.at(id).last_reset = time;
+      33      182222 :   }
+      34             : 
+      35           9 :   void TimeoutManager::pause(const timeout_id_t id)
+      36             :   {
+      37           9 :     std::scoped_lock lck(m_mtx);
+      38           9 :     m_timeouts.at(id).started = false;
+      39           9 :   }
+      40             : 
+      41          99 :   void TimeoutManager::start(const timeout_id_t id, const ros::Time& time)
+      42             :   {
+      43          99 :     std::scoped_lock lck(m_mtx);
+      44          99 :     m_timeouts.at(id).started = true;
+      45          99 :     m_timeouts.at(id).last_reset = time;
+      46          99 :   }
+      47             : 
+      48           2 :   void TimeoutManager::pauseAll()
+      49             :   {
+      50           4 :     std::scoped_lock lck(m_mtx);
+      51          10 :     for (auto& timeout_info : m_timeouts)
+      52           8 :       timeout_info.started = false;
+      53           2 :   }
+      54             : 
+      55           4 :   void TimeoutManager::startAll(const ros::Time& time)
+      56             :   {
+      57           8 :     std::scoped_lock lck(m_mtx);
+      58          20 :     for (auto& timeout_info : m_timeouts)
+      59             :     {
+      60          16 :       timeout_info.started = true;
+      61          16 :       timeout_info.last_reset = time;
+      62             :     }
+      63           4 :   }
+      64             : 
+      65           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)
+      66             :   {
+      67           0 :     std::scoped_lock lck(m_mtx);
+      68           0 :     auto& timeout_info = m_timeouts.at(id);
+      69           0 :     timeout_info.oneshot = oneshot;
+      70           0 :     timeout_info.started = autostart;
+      71           0 :     timeout_info.timeout = timeout;
+      72           0 :     timeout_info.callback = callback;
+      73           0 :     timeout_info.last_reset = last_reset;
+      74           0 :   }
+      75             : 
+      76           0 :   ros::Time TimeoutManager::lastReset(const timeout_id_t id)
+      77             :   {
+      78           0 :     std::scoped_lock lck(m_mtx);
+      79           0 :     return m_timeouts.at(id).last_reset;
+      80             :   }
+      81             : 
+      82        1402 :   bool TimeoutManager::started(const timeout_id_t id)
+      83             :   {
+      84        1402 :     std::scoped_lock lck(m_mtx);
+      85        2804 :     return m_timeouts.at(id).started;
+      86             :   }
+      87             : 
+      88       20854 :   void TimeoutManager::main_timer_callback([[maybe_unused]] const ros::TimerEvent &evt)
+      89             :   {
+      90       20854 :     const auto now = ros::Time::now();
+      91             : 
+      92       41708 :     std::scoped_lock lck(m_mtx);
+      93       44993 :     for (auto& timeout_info : m_timeouts)
+      94             :     {
+      95             :       // don't worry, these variables will get optimized out by the compiler
+      96       24139 :       const bool started = timeout_info.started;
+      97       24139 :       const bool last_reset_timeout = now - timeout_info.last_reset >= timeout_info.timeout;
+      98       24139 :       const bool last_callback_timeout = now - timeout_info.last_callback >= timeout_info.timeout;
+      99       24139 :       if (started && last_reset_timeout && last_callback_timeout)
+     100             :       {
+     101        2023 :         timeout_info.callback(timeout_info.last_reset);
+     102        2023 :         timeout_info.last_callback = now;
+     103             :         // if the timeout is oneshot, pause it
+     104        2023 :         if (timeout_info.oneshot)
+     105           0 :           timeout_info.started = false;
+     106             :       }
+     107             :     }
+     108       20854 :   }
+     109             : }
+
+
+
+ + + + +
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..1e6f9167ba --- /dev/null +++ b/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.overview.html @@ -0,0 +1,48 @@ + + + + + + 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 + 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..93368abc828ff5022496ae27bdf81b1ffbc2202e GIT binary patch literal 539 zcmV+$0_6RPP)5%B{L~QFqm2 z#;oe#7q%h zK0G|`GL9@@yKuSGk7q_7$9AN$yUT{l+qoq4u2~|q4yxSkO<-V)HdI&=jUGvuQUVF& zn8tl)YV^n|v?q>SV3M{njbqduDZJEA70jm|KYS3+;kO=0jCTLUk?Yb^!{NNoYkRUk zCm#yfq4AGevk{zzCp*E;ev+NsfNU%k=21rlW28OYdE;ztQ9yz(1=`)6H{EJWKp?AE2=*Tk d?5)7W$sdv`nsbqF=P&>O002ovPDHLkV1k<^_}%~j literal 0 HcmV?d00001 diff --git a/mrs_lib/src/timer/index-detail-sort-f.html b/mrs_lib/src/timer/index-detail-sort-f.html new file mode 100644 index 0000000000..42d64da8b5 --- /dev/null +++ b/mrs_lib/src/timer/index-detail-sort-f.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-12-17 22:35:29Functions: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..1f0f1e8d68 --- /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-12-17 22:35:29Functions: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..8b5cd047ea --- /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-12-17 22:35:29Functions: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..c517265ce5 --- /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-12-17 22:35:29Functions: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..dadc53878f --- /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-12-17 22:35:29Functions: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..08a7e63818 --- /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-12-17 22:35:29Functions: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..6f0c293ccb --- /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-12-17 22:35:29Functions: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..feff6a2f8e --- /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-12-17 22:35:29Functions: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..6af43dd706 --- /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-12-17 22:35:29Functions: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..247b7ad835 --- /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-12-17 22:35:29Functions: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..5ff4cc7455 --- /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-12-17 22:35:29Functions: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..c46da527f8 --- /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-12-17 22:35:29Functions: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..9c3c2383eb --- /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-12-17 22:35:29Functions: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..4614b0d750 --- /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-12-17 22:35:29Functions: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..76a9c293f0 --- /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-12-17 22:35:29Functions: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..0bce5fb617 --- /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-12-17 22:35:29Functions: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()208
mrs_lib::TransformBroadcaster::sendTransform(geometry_msgs::TransformStamped_<std::allocator<void> > const&)2051826
+
+
+ + + +
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..f0d897b564 --- /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-12-17 22:35:29Functions: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&)2051826
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()208
+
+
+ + + +
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..b54cd78e98 --- /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-12-17 22:35:29Functions: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         208 : TransformBroadcaster::TransformBroadcaster() {
+       8         208 :   broadcaster_ = tf2_ros::TransformBroadcaster();
+       9         208 : }
+      10             : //}
+      11             : 
+      12             : /* sendTransform (const geometry_msgs::TransformStamped &transform) //{ */
+      13     2051826 : void TransformBroadcaster::sendTransform(const geometry_msgs::TransformStamped &transform) {
+      14     4101844 :   std::string frames_from_to = transform.header.frame_id + transform.child_frame_id;
+      15     2048648 :   if (last_messages_.count(frames_from_to) > 0) {
+      16     2049997 :     if (transform.header.stamp > last_messages_[frames_from_to]) {
+      17     1730619 :       broadcaster_.sendTransform(transform);
+      18     1733441 :       last_messages_[frames_from_to] = transform.header.stamp;
+      19             :     } else {
+      20      315599 :       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        2124 :     std::pair<std::string, ros::Time> new_pair;
+      25        1065 :     new_pair.first  = frames_from_to;
+      26        1064 :     new_pair.second = transform.header.stamp;
+      27        1064 :     broadcaster_.sendTransform(transform);
+      28        1065 :     last_messages_.insert(new_pair);
+      29             :   }
+      30     2052116 : }
+      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-12-17 22:35:29Functions: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..7f61f09e04 --- /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-12-17 22:35:29Functions: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..82eb05edef --- /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-12-17 22:35:29Functions: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..acda5dc53f --- /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-12-17 22:35:29Functions: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..f9b39bdbf9 --- /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-12-17 22:35:29Functions: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..81ae201de8 --- /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-12-17 22:35:29Functions: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..78dc85d86c --- /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-12-17 22:35:29Functions: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&)758
mrs_lib::Transformer::UTMtoLL(geometry_msgs::PoseStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2190
mrs_lib::Transformer::UTMtoLL(geometry_msgs::Pose_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2190
mrs_lib::Transformer::UTMtoLL(geometry_msgs::Point_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2192
mrs_lib::Transformer::getFramePrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)4390
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&)24841
mrs_lib::Transformer::transformImpl(geometry_msgs::TransformStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)153275
mrs_lib::Transformer::setLatLon(double, double)159931
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&)1773160
mrs_lib::Transformer::resolveFrameImpl(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)7459284
+
+
+ + + +
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..7e7a401171 --- /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-12-17 22:35:29Functions: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&)24841
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&)153275
mrs_lib::Transformer::getFramePrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)4390
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&)1773160
mrs_lib::Transformer::resolveFrameImpl(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)7459284
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&)2190
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&)2190
mrs_lib::Transformer::UTMtoLL(geometry_msgs::Point_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2192
mrs_lib::Transformer::setLatLon(double, double)159931
mrs_lib::Transformer::Transformer(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Duration const&)758
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..c9dd2b65ae --- /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-12-17 22:35:29Functions: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         758 :   Transformer::Transformer(const ros::NodeHandle& nh, const std::string& node_name, const ros::Duration& cache_time)
+      49         758 :     : 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         758 :   }
+      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       24841 :   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       49688 :     std::scoped_lock lck(mutex_);
+      81             : 
+      82       24847 :     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       49694 :     const std::string from_frame = resolveFrameImpl(from_frame_raw);
+      90       49694 :     const std::string to_frame = resolveFrameImpl(to_frame_raw);
+      91       49694 :     const std::string latlon_frame = resolveFrameImpl(LATLON_ORIGIN);
+      92             : 
+      93       24847 :     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      159931 :   void Transformer::setLatLon(const double lat, const double lon)
+     118             :   {
+     119      159931 :     std::scoped_lock lck(mutex_);
+     120             : 
+     121             :     double utm_x, utm_y;
+     122      159447 :     mrs_lib::LLtoUTM(lat, lon, utm_y, utm_x, utm_zone_.data());
+     123      158755 :     got_utm_zone_ = true;
+     124      158618 :   }
+     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      153275 :   std::optional<mrs_msgs::ReferenceStamped> Transformer::transformImpl(const geometry_msgs::TransformStamped& tf, const mrs_msgs::ReferenceStamped& what)
+     253             :   {
+     254             :     // create a pose message
+     255      306550 :     geometry_msgs::PoseStamped pose;
+     256      153275 :     pose.header = what.header;
+     257             :   
+     258      153275 :     pose.pose.position.x = what.reference.position.x;
+     259      153275 :     pose.pose.position.y = what.reference.position.y;
+     260      153275 :     pose.pose.position.z = what.reference.position.z;
+     261      153275 :     pose.pose.orientation = geometry::fromEigen(geometry::quaternionFromHeading(what.reference.heading));
+     262             :   
+     263             :     // try to transform the pose message
+     264      306550 :     const auto pose_opt = transformImpl(tf, pose);
+     265      153275 :     if (!pose_opt.has_value())
+     266           0 :       return std::nullopt;
+     267             :     // overwrite the pose with it's transformed value
+     268      153275 :     pose = pose_opt.value();
+     269             :   
+     270      306550 :     mrs_msgs::ReferenceStamped ret;
+     271      153275 :     ret.header = pose.header;
+     272             :   
+     273             :     // copy the new transformed data back
+     274      153275 :     ret.reference.position.x = pose.pose.position.x;
+     275      153275 :     ret.reference.position.y = pose.pose.position.y;
+     276      153275 :     ret.reference.position.z = pose.pose.position.z;
+     277      153275 :     ret.reference.heading = geometry::headingFromRot(geometry::toEigen(pose.pose.orientation));
+     278      153275 :     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     1773160 :   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     1773160 :     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     1773160 :     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     1773146 :     if (from_frame == to_frame)
+     319      454530 :       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     1545889 :     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     1545882 :     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        4386 :       const std::string utm_frame = getFramePrefix(to_frame) + "utm_origin";
+     337        4386 :       auto tf_opt = getTransformImpl(from_frame, utm_frame, time_stamp, latlon_frame);
+     338        2193 :       if (!tf_opt.has_value())
+     339           0 :         return std::nullopt;
+     340             :       // change the transformation frames to point to latlon
+     341        2193 :       frame_to(*tf_opt) = to_frame;
+     342        2193 :       return tf_opt;
+     343             :     }
+     344             : 
+     345     4631042 :     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     2142016 :       return tf_buffer_->lookupTransform(to_frame, from_frame, time_stamp, lookup_timeout_);
+     351             :     }
+     352     1890664 :     catch (tf2::TransformException& e)
+     353             :     {
+     354      945340 :       ex = e;
+     355             :     }
+     356             : 
+     357             :     // if that failed, try to get the newest one if requested
+     358      945326 :     if (retry_lookup_newest_)
+     359             :     {
+     360             :       try
+     361             :       {
+     362     1876771 :         return tf_buffer_->lookupTransform(to_frame, from_frame, ros::Time(0), lookup_timeout_);
+     363             :       }
+     364       27570 :       catch (tf2::TransformException& e)
+     365             :       {
+     366       13785 :         ex = e;
+     367             :       }
+     368             :     }
+     369             : 
+     370             :     // if the flow got here, we've failed to look the transform up
+     371       13815 :     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       13815 :       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       13815 :     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     7459284 :   std::string Transformer::resolveFrameImpl(const std::string& frame_id)
+     464             :   {
+     465             :     // if the frame is empty, return the default frame id
+     466     7459284 :     if (frame_id.empty())
+     467        6932 :       return default_frame_id_;
+     468             : 
+     469             :     // if there is no prefix set, just return the raw frame id
+     470     7452343 :     if (prefix_.empty())
+     471     4504029 :       return frame_id;
+     472             : 
+     473             :     // if there is a default prefix set and the frame does not start with it, prefix it
+     474     2948199 :     if (frame_id.substr(0, prefix_.length()) != prefix_)
+     475     1857648 :       return prefix_ + frame_id;
+     476             : 
+     477     1090706 :     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, 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        2192 :   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        2192 :     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        2191 :     geometry_msgs::Point latlon;
+     532        2191 :     mrs_lib::UTMtoLL(what.y, what.x, utm_zone_.data(), latlon.x, latlon.y);
+     533        2191 :     latlon.z = what.z;
+     534        2191 :     return latlon;
+     535             :   }
+     536             : 
+     537           0 :   std::optional<geometry_msgs::PointStamped> Transformer::UTMtoLL(const geometry_msgs::PointStamped& what, 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 + LATLON_ORIGIN;
+     545           0 :     ret.header.stamp = what.header.stamp;
+     546           0 :     ret.point = opt.value();
+     547           0 :     return ret;
+     548             :   }
+     549             : 
+     550        2190 :   std::optional<geometry_msgs::Pose> Transformer::UTMtoLL(const geometry_msgs::Pose& what, const std::string& prefix)
+     551             :   {
+     552        2190 :     const auto opt = UTMtoLL(what.position, prefix);
+     553        2190 :     if (!opt.has_value())
+     554           0 :       return std::nullopt;
+     555             : 
+     556        2190 :     geometry_msgs::Pose ret;
+     557        2190 :     ret.position = opt.value();
+     558        2190 :     ret.orientation = what.orientation;
+     559        2190 :     return ret;
+     560             :   }
+     561             : 
+     562        2190 :   std::optional<geometry_msgs::PoseStamped> Transformer::UTMtoLL(const geometry_msgs::PoseStamped& what, const std::string& prefix)
+     563             :   {
+     564        2190 :     const auto opt = UTMtoLL(what.pose, prefix);
+     565        2190 :     if (!opt.has_value())
+     566           0 :       return std::nullopt;
+     567             : 
+     568        4380 :     geometry_msgs::PoseStamped ret;
+     569        2190 :     ret.header.frame_id = prefix + LATLON_ORIGIN;
+     570        2190 :     ret.header.stamp = what.header.stamp;
+     571        2190 :     ret.pose = opt.value();
+     572        2190 :     return ret;
+     573             :   }
+     574             :   //}
+     575             : 
+     576             :   /* getFramePrefix() method //{ */
+     577        4390 :   std::string Transformer::getFramePrefix(const std::string& frame_id)
+     578             :   {
+     579        4390 :     const auto firstof = frame_id.find_first_of('/');
+     580        4390 :     if (firstof == std::string::npos)
+     581           0 :       return "";
+     582             :     else
+     583        4390 :       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..7eed470c785336ad3284e1537c87589ff99ed190 GIT binary patch literal 2003 zcmV;^2Q2uBP))dTBU|#LNWnRdg4=R%6|$^fww&8EOw%~IaqBZI zs$oh)4A=zC0Y^E;1~35XR>lp_#o>58c3#W#F{d+!&K8reEB?A%b&eXSISbgB`U#9} zKn24slBO!8AppS~7l6yz~<#Q989&&>i^;05@WBM^Zg>j;ENwk<7>jRIqId1FT z{?C(u!f~53g+-QV-vrqal8}FAv$V8+*iX{z<1E_>lL?BoYeuZ({QBci%!k13m?PNRR@f@wq^sFu!*MeO5<`x zY1ljC7c-M}7+3_+2u!0@FK%5IBju#Ik0^ZE3t$^fXME(Wp=giLAR3 zf*r6LE~Ak+09s4^CSa{I42Cu;j8QF`Y)1uTnt3TXnneYM;c^GCdZ_K^&;IZ)J^`Sf z0r^LE=999%7w)qJG<+VVj@M%}RwtXPnNk`}GbCROc%Y9Q_b|rp2ecQrNpvakTTNe) zw~>!3M;gr9CqSgn7)@R5Ak`(A96-_s=L|{_A_u_Bx|&l|0pB5~=&xoMp2hXulPbnj z-47qws{mz+Z^sEqr2F7w^8jtnB)Xauh|jZUCf+_BNym4_&qU3&i)7U7nV^LAzAf@5 zI<&|h&zdG3Ro_!-8PaA_mbwAu{vmc-VUD4hIDo!dVbLB7`)(|th)C>v zSYfL$ii9o4c*WJPU{%2zLy8tHEit@a5mgqWPhC^a3W=-BUHvcOeFdE5R<^XzE6E?Xs32SVxDy&N zok^P##qz1eLzua~%h3>ia7YjAGp)w*`Ybjb=}sX1!6RhB1Ej>X-30HY@YmgFV%cU2<9wjeIkivQ0wNnD#!$ z`@`WOY4%{H+-AaC&C{RarF@^oF;kPd+5JaKB>c<2;k*xefUd_n)0C~dp(9hh!Z3q-9ua@nrF=RIlw7~HFw*iCj?g2ZIXst63DVK3 zua1Rn*P?SNt`8Tv+*;L3wn#YSnsWsOjdX>_fW+;!9{(nFdk|!fXnakaDVpRDKq7+M# zd9^|UWbC4FR5-d+)m + + + + + + 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-12-17 22:35:29Functions: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..0ffdb3553f --- /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-12-17 22:35:29Functions: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..b370d70f33 --- /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-12-17 22:35:29Functions: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..2785814657 --- /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-12-17 22:35:29Functions: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..fad821ab31 --- /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-12-17 22:35:29Functions: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..45b9ae1493 --- /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-12-17 22:35:29Functions: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..e6a9c60518 --- /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-12-17 22:35:29Functions: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>&)589632
mrs_lib::AtomicScopeFlag::~AtomicScopeFlag()589638
+
+
+ + + +
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..3a9c9c90ea --- /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-12-17 22:35:29Functions: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>&)589632
mrs_lib::AtomicScopeFlag::~AtomicScopeFlag()589638
+
+
+ + + +
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..f5a5e1530e --- /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-12-17 22:35:29Functions: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      589632 : AtomicScopeFlag::AtomicScopeFlag(std::atomic<bool>& in)
+       7      589632 :   : variable(in)
+       8             : {
+       9      589632 :   variable = true;
+      10      589638 : }
+      11             : 
+      12     1179278 : AtomicScopeFlag::~AtomicScopeFlag()
+      13             : {
+      14      589638 :   variable = false;
+      15      589640 : }
+      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:29333587.5 %
Date:2024-12-17 22:35:29Functions:2222100.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()20
mrs_uav_autostart::automatic_start::AutomaticStart::toggleControlOutput(bool const&)25
(anonymous namespace)::ProxyExec0::ProxyExec0()32
mrs_uav_autostart::automatic_start::AutomaticStart::onInit()32
mrs_uav_autostart::automatic_start::AutomaticStart::changeState(mrs_uav_autostart::automatic_start::LandingStates_t)51
mrs_uav_autostart::automatic_start::Topic::Topic(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)65
mrs_uav_autostart::automatic_start::Topic::getTopicName[abi:cxx11]()75
mrs_uav_autostart::automatic_start::AutomaticStart::callbackHwApiCapabilities(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const>)636
mrs_uav_autostart::automatic_start::AutomaticStart::callbackGazeboSpawnerDiagnostics(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)3475
mrs_uav_autostart::automatic_start::AutomaticStart::topicCheck()10072
mrs_uav_autostart::automatic_start::AutomaticStart::validateReference()10072
mrs_uav_autostart::automatic_start::AutomaticStart::isGazeboSimulation()10072
mrs_uav_autostart::automatic_start::AutomaticStart::preflighCheckGyro()10220
mrs_uav_autostart::automatic_start::AutomaticStart::preflighCheckHeight()10220
mrs_uav_autostart::automatic_start::AutomaticStart::preflightCheckSpeed()10220
mrs_uav_autostart::automatic_start::AutomaticStart::timerMain(ros::TimerEvent const&)19396
mrs_uav_autostart::automatic_start::Topic::getTime()40363
mrs_uav_autostart::automatic_start::Topic::updateTime()50333
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&) const50337
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)50356
mrs_uav_autostart::automatic_start::AutomaticStart::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)124784
+
+
+ + + +
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..f33cd7566a --- /dev/null +++ b/mrs_uav_autostart/src/automatic_start.cpp.func.html @@ -0,0 +1,168 @@ + + + + + + + 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:29333587.5 %
Date:2024-12-17 22:35:29Functions:2222100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()32
mrs_uav_autostart::automatic_start::AutomaticStart::topicCheck()10072
mrs_uav_autostart::automatic_start::AutomaticStart::changeState(mrs_uav_autostart::automatic_start::LandingStates_t)51
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)50356
mrs_uav_autostart::automatic_start::AutomaticStart::preflighCheckGyro()10220
mrs_uav_autostart::automatic_start::AutomaticStart::validateReference()10072
mrs_uav_autostart::automatic_start::AutomaticStart::isGazeboSimulation()10072
mrs_uav_autostart::automatic_start::AutomaticStart::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)124784
mrs_uav_autostart::automatic_start::AutomaticStart::preflighCheckHeight()10220
mrs_uav_autostart::automatic_start::AutomaticStart::preflightCheckSpeed()10220
mrs_uav_autostart::automatic_start::AutomaticStart::toggleControlOutput(bool const&)25
mrs_uav_autostart::automatic_start::AutomaticStart::callbackHwApiCapabilities(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const>)636
mrs_uav_autostart::automatic_start::AutomaticStart::callbackGazeboSpawnerDiagnostics(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)3475
mrs_uav_autostart::automatic_start::AutomaticStart::disarm()2
mrs_uav_autostart::automatic_start::AutomaticStart::onInit()32
mrs_uav_autostart::automatic_start::AutomaticStart::takeoff()20
mrs_uav_autostart::automatic_start::AutomaticStart::timerMain(ros::TimerEvent const&)19396
mrs_uav_autostart::automatic_start::Topic::updateTime()50333
mrs_uav_autostart::automatic_start::Topic::getTopicName[abi:cxx11]()75
mrs_uav_autostart::automatic_start::Topic::getTime()40363
mrs_uav_autostart::automatic_start::Topic::Topic(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)65
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&) const50337
+
+
+ + + +
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..92bfe4c54d --- /dev/null +++ b/mrs_uav_autostart/src/automatic_start.cpp.gcov.html @@ -0,0 +1,1078 @@ + + + + + + + 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:29333587.5 %
Date:2024-12-17 22:35:29Functions:2222100.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          65 :   Topic(std::string topic_name) : topic_name_(topic_name) {
+      46          65 :     last_time_ = ros::Time::UNINITIALIZED;
+      47          65 :   }
+      48             : 
+      49       50333 :   void updateTime(void) {
+      50       50333 :     last_time_ = ros::Time::now();
+      51       50350 :   }
+      52             : 
+      53       40363 :   ros::Time getTime(void) {
+      54       40363 :     return last_time_;
+      55             :   }
+      56             : 
+      57          75 :   std::string getTopicName(void) {
+      58          75 :     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 ------------------------- |
+     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             :   void callbackHwApiCapabilities(const mrs_msgs::HwApiCapabilities::ConstPtr msg);
+     123             : 
+     124             :   // | --------------- Gazebo spawner diagnostics --------------- |
+     125             : 
+     126             :   void                               callbackGazeboSpawnerDiagnostics(const mrs_msgs::GazeboSpawnerDiagnostics::ConstPtr msg);
+     127             :   std::atomic<bool>                  got_gazebo_spawner_diagnostics = false;
+     128             :   mrs_msgs::GazeboSpawnerDiagnostics gazebo_spawner_diagnostics_;
+     129             :   std::mutex                         mutex_gazebo_spawner_diagnostics_;
+     130             : 
+     131             :   // | ----------------- arm and offboard check ----------------- |
+     132             : 
+     133             :   ros::Time armed_time_;
+     134             :   bool      armed_ = false;
+     135             : 
+     136             :   ros::Time offboard_time_;
+     137             :   bool      offboard_ = false;
+     138             : 
+     139             :   bool we_toggled_output_ = false;
+     140             : 
+     141             :   // | ------------------------ routines ------------------------ |
+     142             : 
+     143             :   bool takeoff();
+     144             : 
+     145             :   bool validateReference();
+     146             : 
+     147             :   bool toggleControlOutput(const bool& value);
+     148             :   bool disarm();
+     149             : 
+     150             :   bool isGazeboSimulation(void);
+     151             :   bool topicCheck(void);
+     152             :   bool preflightCheckSpeed(void);
+     153             :   bool preflighCheckHeight(void);
+     154             :   bool preflighCheckGyro(void);
+     155             : 
+     156             :   bool is_gazebo_simulation_ = false;
+     157             : 
+     158             :   // | ---------------------- other params ---------------------- |
+     159             : 
+     160             :   std::string _body_frame_name_;
+     161             :   double      _pre_takeoff_sleep_;
+     162             :   bool        _handle_takeoff_ = false;
+     163             :   double      _safety_timeout_;
+     164             :   double      _control_output_timeout_;
+     165             : 
+     166             :   // | ---------------------- state machine --------------------- |
+     167             : 
+     168             :   uint current_state = STATE_IDLE;
+     169             :   void changeState(LandingStates_t new_state);
+     170             : 
+     171             :   // | --------------------- preflight check -------------------- |
+     172             : 
+     173             :   double _preflight_check_time_window_;
+     174             : 
+     175             :   // | ------------------ preflight speed check ----------------- |
+     176             : 
+     177             :   bool      _speed_check_enabled_ = false;
+     178             :   double    _speed_check_max_speed_;
+     179             :   ros::Time speed_check_violated_time_;
+     180             : 
+     181             :   // | ----------------- preflight height check ----------------- |
+     182             : 
+     183             :   bool      _height_check_enabled_ = false;
+     184             :   double    _height_check_max_height_;
+     185             :   ros::Time height_check_violated_time_;
+     186             : 
+     187             :   // | ----------------- preflight gyro check ----------------- |
+     188             : 
+     189             :   bool      _gyro_check_enabled_ = false;
+     190             :   double    _gyro_check_max_rate_;
+     191             :   ros::Time gyro_check_violated_time_;
+     192             : 
+     193             :   // | ---------------- generic topic subscribers --------------- |
+     194             : 
+     195             :   bool                     _topic_check_enabled_ = false;
+     196             :   double                   _topic_check_timeout_;
+     197             :   std::vector<std::string> _topic_check_topic_names_;
+     198             : 
+     199             :   std::vector<Topic>           topic_check_topics_;
+     200             :   std::vector<ros::Subscriber> generic_subscriber_vec_;
+     201             : 
+     202             :   // generic callback, for any topic, to monitor its rate
+     203             :   void genericCallback(const topic_tools::ShapeShifter::ConstPtr& msg, const std::string& topic_name, const int id);
+     204             : };
+     205             : 
+     206             : //}
+     207             : 
+     208             : /* onInit() //{ */
+     209             : 
+     210          32 : void AutomaticStart::onInit() {
+     211             : 
+     212          32 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     213             : 
+     214          32 :   ros::Time::waitForValid();
+     215             : 
+     216          32 :   armed_      = false;
+     217          32 :   armed_time_ = ros::Time(0);
+     218             : 
+     219          32 :   offboard_      = false;
+     220          32 :   offboard_time_ = ros::Time(0);
+     221             : 
+     222          64 :   mrs_lib::ParamLoader param_loader(nh_, "AutomaticStart");
+     223             : 
+     224          64 :   std::string custom_config_path;
+     225             : 
+     226          32 :   param_loader.loadParam("custom_config", custom_config_path);
+     227             : 
+     228          32 :   if (custom_config_path != "") {
+     229           5 :     param_loader.addYamlFile(custom_config_path);
+     230             :   }
+     231             : 
+     232          32 :   param_loader.addYamlFileFromParam("config_private");
+     233          32 :   param_loader.addYamlFileFromParam("config_public");
+     234             : 
+     235          32 :   param_loader.loadParam("uav_name", _uav_name_);
+     236          32 :   param_loader.loadParam("simulation", _simulation_);
+     237             : 
+     238          32 :   param_loader.loadParam("main_timer_rate", _main_timer_rate_);
+     239          32 :   param_loader.loadParam("body_frame_name", _body_frame_name_);
+     240          32 :   param_loader.loadParam("control_output_timeout", _control_output_timeout_);
+     241             : 
+     242          32 :   param_loader.loadParam("safety_timeout", _safety_timeout_);
+     243          32 :   param_loader.loadParam("pre_takeoff_sleep", _pre_takeoff_sleep_);
+     244             : 
+     245          32 :   param_loader.loadParam("handle_takeoff", _handle_takeoff_);
+     246             : 
+     247          32 :   param_loader.loadParam("preflight_check/time_window", _preflight_check_time_window_);
+     248             : 
+     249          32 :   param_loader.loadParam("preflight_check/speed_check/enabled", _speed_check_enabled_);
+     250          32 :   param_loader.loadParam("preflight_check/speed_check/max_speed", _speed_check_max_speed_);
+     251             : 
+     252          32 :   param_loader.loadParam("preflight_check/height_check/enabled", _height_check_enabled_);
+     253          32 :   param_loader.loadParam("preflight_check/height_check/max_height", _height_check_max_height_);
+     254             : 
+     255          32 :   param_loader.loadParam("preflight_check/gyro_check/enabled", _gyro_check_enabled_);
+     256          32 :   param_loader.loadParam("preflight_check/gyro_check/max_rate", _gyro_check_max_rate_);
+     257             : 
+     258          32 :   param_loader.loadParam("preflight_check/topic_check/enabled", _topic_check_enabled_);
+     259          32 :   param_loader.loadParam("preflight_check/topic_check/timeout", _topic_check_timeout_);
+     260          32 :   param_loader.loadParam("preflight_check/topic_check/topics", _topic_check_topic_names_);
+     261             : 
+     262          32 :   if (!param_loader.loadedSuccessfully()) {
+     263           0 :     ROS_ERROR("[AutomaticStart]: Could not load all parameters!");
+     264           0 :     ros::shutdown();
+     265             :   }
+     266             : 
+     267             :   // | ----------------------- subscribers ---------------------- |
+     268             : 
+     269          64 :   mrs_lib::SubscribeHandlerOptions shopts;
+     270          32 :   shopts.nh                 = nh_;
+     271          32 :   shopts.node_name          = "AutomaticStart";
+     272          32 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     273          32 :   shopts.threadsafe         = true;
+     274          32 :   shopts.autostart          = true;
+     275          32 :   shopts.queue_size         = 10;
+     276          32 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     277             : 
+     278          32 :   sh_estimation_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, "estimation_diag_in");
+     279          32 :   sh_hw_api_status_   = mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus>(shopts, "hw_api_status_in", &AutomaticStart::callbackHwApiStatus, this);
+     280             :   sh_hw_api_capabilities_ =
+     281          32 :       mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities>(shopts, "hw_api_capabilities_in", &AutomaticStart::callbackHwApiCapabilities, this);
+     282          32 :   sh_distance_sensor_      = mrs_lib::SubscribeHandler<sensor_msgs::Range>(shopts, "distance_sensor_in");
+     283          32 :   sh_imu_                  = mrs_lib::SubscribeHandler<sensor_msgs::Imu>(shopts, "imu_in");
+     284          32 :   sh_control_manager_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>(shopts, "control_manager_diagnostics_in");
+     285          32 :   sh_uav_manager_diag_     = mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics>(shopts, "uav_manager_diagnostics_in");
+     286          64 :   sh_gazebo_spawner_diag_  = mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics>(shopts, "gazebo_spawner_diagnostics_in",
+     287          32 :                                                                                           &AutomaticStart::callbackGazeboSpawnerDiagnostics, this);
+     288             : 
+     289             :   // | ----------------------- publishers ----------------------- |
+     290             : 
+     291          32 :   ph_can_takeoff_ = mrs_lib::PublisherHandler<std_msgs::Bool>(nh_, "can_takeoff_out");
+     292             : 
+     293             :   // | --------------------- service clients -------------------- |
+     294             : 
+     295          32 :   service_client_takeoff_               = nh_.serviceClient<std_srvs::Trigger>("takeoff_out");
+     296          32 :   service_client_toggle_control_output_ = nh_.serviceClient<std_srvs::SetBool>("toggle_control_output_out");
+     297          32 :   service_client_arm_                   = nh_.serviceClient<std_srvs::SetBool>("arm_out");
+     298             : 
+     299          32 :   service_client_validate_reference_ = nh_.serviceClient<mrs_msgs::ValidateReference>("validate_reference_out");
+     300             : 
+     301             :   // | ------------------ setup generic topics ------------------ |
+     302             : 
+     303          32 :   if (_topic_check_enabled_) {
+     304             : 
+     305          64 :     boost::function<void(const topic_tools::ShapeShifter::ConstPtr&)> callback;
+     306             : 
+     307          97 :     for (int i = 0; i < int(_topic_check_topic_names_.size()); i++) {
+     308             : 
+     309         130 :       std::string topic_name = _topic_check_topic_names_.at(i);
+     310             : 
+     311          65 :       if (topic_name.at(0) != '/') {
+     312          65 :         topic_name = "/" + _uav_name_ + "/" + topic_name;
+     313             :       }
+     314             : 
+     315         130 :       Topic tmp_topic(topic_name);
+     316          65 :       topic_check_topics_.push_back(tmp_topic);
+     317             : 
+     318          65 :       int id = i;  // id to identify which topic called the generic callback
+     319             : 
+     320       50402 :       callback                       = [this, topic_name, id](const topic_tools::ShapeShifter::ConstPtr& msg) -> void { genericCallback(msg, topic_name, id); };
+     321         195 :       ros::Subscriber tmp_subscriber = nh_.subscribe(topic_name, 1, callback);
+     322             : 
+     323          65 :       generic_subscriber_vec_.push_back(tmp_subscriber);
+     324             :     }
+     325             :   }
+     326             : 
+     327             :   // | ------------------------- timers ------------------------- |
+     328             : 
+     329          32 :   timer_main_ = nh_.createTimer(ros::Rate(_main_timer_rate_), &AutomaticStart::timerMain, this);
+     330             : 
+     331             :   // | --------------------- finish the init -------------------- |
+     332             : 
+     333          32 :   is_initialized_ = true;
+     334             : 
+     335          32 :   ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: initialized");
+     336          32 : }
+     337             : 
+     338             : //}
+     339             : 
+     340             : // --------------------------------------------------------------
+     341             : // |                          callbacks                         |
+     342             : // --------------------------------------------------------------
+     343             : 
+     344             : /* genericCallback() //{ */
+     345             : 
+     346       50356 : void AutomaticStart::genericCallback([[maybe_unused]] const topic_tools::ShapeShifter::ConstPtr& msg, [[maybe_unused]] const std::string& topic_name,
+     347             :                                      const int id) {
+     348       50356 :   topic_check_topics_.at(id).updateTime();
+     349       50351 : }
+     350             : 
+     351             : //}
+     352             : 
+     353             : /* callbackHwApiStatus() //{ */
+     354             : 
+     355      124784 : void AutomaticStart::callbackHwApiStatus(const mrs_msgs::HwApiStatus::ConstPtr msg) {
+     356             : 
+     357      124784 :   if (!is_initialized_) {
+     358           0 :     return;
+     359             :   }
+     360             : 
+     361      124784 :   ROS_INFO_ONCE("[AutomaticStart]: getting HW API status");
+     362             : 
+     363      249568 :   std::scoped_lock lock(mutex_hw_api_status_);
+     364             : 
+     365             :   // check armed_ state
+     366      124784 :   if (armed_ == false) {
+     367             : 
+     368             :     // if armed_ state changed to true, please "start the clock"
+     369       72192 :     if (msg->armed) {
+     370             : 
+     371          31 :       armed_      = true;
+     372          31 :       armed_time_ = ros::Time::now();
+     373             :     }
+     374             : 
+     375             :     // if we were armed_ previously
+     376       52592 :   } else if (armed_ == true) {
+     377             : 
+     378             :     // and we are not really now
+     379       52592 :     if (!msg->armed) {
+     380             : 
+     381           2 :       armed_ = false;
+     382             :     }
+     383             :   }
+     384             : 
+     385             :   // check offboard_ state
+     386      124784 :   if (offboard_ == false) {
+     387             : 
+     388             :     // if offboard_ state changed to true, please "start the clock"
+     389       83113 :     if (msg->offboard) {
+     390             : 
+     391          23 :       offboard_      = true;
+     392          23 :       offboard_time_ = ros::Time::now();
+     393             :     }
+     394             : 
+     395             :     // if we were in offboard_ previously
+     396       41671 :   } else if (offboard_ == true) {
+     397             : 
+     398             :     // and we are not really now
+     399       41671 :     if (!msg->offboard) {
+     400             : 
+     401           0 :       offboard_ = false;
+     402             :     }
+     403             :   }
+     404             : 
+     405      124784 :   if (msg->connected) {
+     406      120799 :     hw_api_connected_ = true;
+     407             :   }
+     408             : }
+     409             : 
+     410             : //}
+     411             : 
+     412             : /* callbackHwApiCapabilities() //{ */
+     413             : 
+     414         636 : void AutomaticStart::callbackHwApiCapabilities([[maybe_unused]] const mrs_msgs::HwApiCapabilities::ConstPtr msg) {
+     415             : 
+     416         636 :   if (!is_initialized_) {
+     417           0 :     return;
+     418             :   }
+     419             : 
+     420         636 :   ROS_INFO_ONCE("[AutomaticStart]: getting HW API capabilities");
+     421             : }
+     422             : 
+     423             : //}
+     424             : 
+     425             : /* callbackGazeboSpawnerDiagnostics() //{ */
+     426             : 
+     427        3475 : void AutomaticStart::callbackGazeboSpawnerDiagnostics(const mrs_msgs::GazeboSpawnerDiagnostics::ConstPtr msg) {
+     428             : 
+     429        3475 :   if (!is_initialized_) {
+     430           0 :     return;
+     431             :   }
+     432             : 
+     433        3475 :   ROS_INFO_ONCE("[AutomaticStart]: getting spawner diagnostics");
+     434             : 
+     435             :   {
+     436        6950 :     std::scoped_lock lock(mutex_gazebo_spawner_diagnostics_);
+     437             : 
+     438        3475 :     gazebo_spawner_diagnostics_ = *msg;
+     439             : 
+     440        3475 :     got_gazebo_spawner_diagnostics = true;
+     441             :   }
+     442             : }
+     443             : 
+     444             : //}
+     445             : 
+     446             : // --------------------------------------------------------------
+     447             : // |                           timers                           |
+     448             : // --------------------------------------------------------------
+     449             : 
+     450             : /* timerMain() //{ */
+     451             : 
+     452       19396 : void AutomaticStart::timerMain([[maybe_unused]] const ros::TimerEvent& event) {
+     453             : 
+     454       19396 :   if (!is_initialized_) {
+     455        6256 :     return;
+     456             :   }
+     457             : 
+     458       19396 :   bool got_uav_manager_diag     = sh_uav_manager_diag_.hasMsg();
+     459       19396 :   bool got_control_manager_diag = sh_control_manager_diag_.hasMsg();
+     460       19396 :   bool got_estimation_diag      = sh_estimation_diag_.hasMsg();
+     461       19396 :   bool got_hw_api               = sh_hw_api_status_.hasMsg() && sh_hw_api_capabilities_.hasMsg() && hw_api_connected_;
+     462             : 
+     463       19396 :   if (!got_control_manager_diag || !got_hw_api || !got_uav_manager_diag || !got_estimation_diag) {
+     464        6108 :     ROS_WARN_THROTTLE(5.0, "[AutomaticStart]: waiting for data: ControlManager=%s, UavManager=%s, HW Api=%s, EstimationManager=%s",
+     465             :                       got_control_manager_diag ? "true" : "FALSE", got_uav_manager_diag ? "true" : "FALSE", got_hw_api ? "true" : "FALSE",
+     466             :                       got_estimation_diag ? "true" : "FALSE");
+     467        6108 :     return;
+     468             :   }
+     469             : 
+     470       13288 :   auto [armed, offboard, armed_time, offboard_time] = mrs_lib::get_mutexed(mutex_hw_api_status_, armed_, offboard_, armed_time_, offboard_time_);
+     471       13288 :   auto control_manager_diagnostics                  = sh_control_manager_diag_.getMsg();
+     472             : 
+     473       13288 :   switch (current_state) {
+     474             : 
+     475       10220 :     case STATE_IDLE: {
+     476             : 
+     477             :       // | --------------------- preflight check -------------------- |
+     478             : 
+     479       10220 :       bool speed_valid  = preflightCheckSpeed();
+     480       10220 :       bool height_valid = preflighCheckHeight();
+     481       10220 :       bool gyros_valid  = preflighCheckGyro();
+     482             : 
+     483       10220 :       bool possibly_in_the_air = !speed_valid || !height_valid || !gyros_valid;
+     484             : 
+     485       10220 :       if (!offboard && possibly_in_the_air) {
+     486             : 
+     487         148 :         ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: preflight check failed, the UAV is possibly in the air");
+     488             : 
+     489         148 :         if (armed) {
+     490             : 
+     491           6 :           ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: -- the UAV is also armed!! finishing to prevent unwanted system activation");
+     492             : 
+     493           6 :           if (we_toggled_output_) {
+     494             : 
+     495           1 :             bool res = toggleControlOutput(false);
+     496             : 
+     497           1 :             if (!res) {
+     498           0 :               ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: could not set control output OFF");
+     499             :             }
+     500             :           }
+     501             : 
+     502           6 :           changeState(STATE_FINISHED);
+     503             : 
+     504         148 :           return;
+     505             :         }
+     506             : 
+     507         142 :         return;
+     508             :       }
+     509             : 
+     510             :       // | -------------------- ready to takeoff -------------------- |
+     511             : 
+     512       10072 :       bool control_output_enabled = sh_control_manager_diag_.getMsg()->output_enabled;
+     513             : 
+     514       10072 :       std_msgs::Bool can_takeoff_msg;
+     515       10072 :       can_takeoff_msg.data = false;
+     516             : 
+     517             :       // | -------------------- preflight checks -------------------- |
+     518             : 
+     519       10072 :       bool position_valid = validateReference();
+     520       10072 :       bool got_topics     = topicCheck();
+     521             : 
+     522       10072 :       bool can_takeoff = got_topics && position_valid;
+     523             : 
+     524             :       // | ---------------------------------------------------------- |
+     525             : 
+     526       10072 :       can_takeoff_msg.data = can_takeoff;
+     527       10072 :       ph_can_takeoff_.publish(can_takeoff_msg);
+     528             : 
+     529       10072 :       if (armed && !control_output_enabled) {
+     530             : 
+     531         117 :         if (can_takeoff) {
+     532             : 
+     533          24 :           bool res = toggleControlOutput(true);
+     534             : 
+     535          24 :           if (!res) {
+     536           0 :             ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: could not set control output ON");
+     537             :           } else {
+     538          24 :             we_toggled_output_ = true;
+     539             :           }
+     540             :         }
+     541             : 
+     542         117 :         double time_from_arming = (ros::Time::now() - armed_time).toSec();
+     543             : 
+     544         117 :         if (armed_time != ros::Time::UNINITIALIZED && time_from_arming > _control_output_timeout_) {
+     545             : 
+     546           2 :           ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: could not set control output ON for %.2f secs, disarming", _control_output_timeout_);
+     547           2 :           disarm();
+     548           2 :           changeState(STATE_FINISHED);
+     549             :         }
+     550             :       }
+     551             : 
+     552       10072 :       if (_simulation_ && isGazeboSimulation()) {
+     553             : 
+     554        6755 :         std::scoped_lock lock(mutex_gazebo_spawner_diagnostics_);
+     555             : 
+     556        6755 :         if (got_gazebo_spawner_diagnostics) {
+     557             : 
+     558        6755 :           if (!gazebo_spawner_diagnostics_.spawn_called || gazebo_spawner_diagnostics_.processing) {
+     559           0 :             ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: (simulation) waiting for spawner to finish spawning UAVs");
+     560           0 :             return;
+     561             :           }
+     562             : 
+     563             :         } else {
+     564             : 
+     565           0 :           ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: (simulation) missing spawner diagnostics");
+     566           0 :           return;
+     567             :         }
+     568             :       }
+     569             : 
+     570             :       // when armed and in offboard, takeoff
+     571       10072 :       if (armed && offboard && control_output_enabled) {
+     572             : 
+     573        3032 :         if (!_handle_takeoff_) {
+     574           3 :           changeState(STATE_FINISHED);
+     575             :         } else {
+     576             : 
+     577        3029 :           ros::Duration armed_time_diff    = ros::Time::now() - armed_time;
+     578        3029 :           ros::Duration offboard_time_diff = ros::Time::now() - offboard_time;
+     579             : 
+     580        3029 :           if (armed_time_diff > ros::Duration(_safety_timeout_) && offboard_time_diff > ros::Duration(_safety_timeout_)) {
+     581             : 
+     582          20 :             changeState(STATE_TAKEOFF);
+     583             : 
+     584             :           } else {
+     585             : 
+     586        3009 :             double min = (armed_time_diff < offboard_time_diff) ? armed_time_diff.toSec() : offboard_time_diff.toSec();
+     587             : 
+     588        3009 :             ROS_WARN_THROTTLE(1.0, "taking off in %.0f", (_safety_timeout_ - min));
+     589             :           }
+     590             :         }
+     591             :       }
+     592             : 
+     593       10072 :       break;
+     594             :     }
+     595             : 
+     596        3037 :     case STATE_TAKEOFF: {
+     597             : 
+     598             :       // if takeoff finished
+     599        3037 :       if (control_manager_diagnostics->flying_normally) {
+     600             : 
+     601          20 :         ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: takeoff finished");
+     602             : 
+     603          20 :         changeState(STATE_FINISHED);
+     604             : 
+     605             :       } else {
+     606             : 
+     607        3017 :         ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: waiting for the takeoff to finish");
+     608             :       }
+     609             : 
+     610        3037 :       break;
+     611             :     }
+     612             : 
+     613          31 :     case STATE_FINISHED: {
+     614             : 
+     615          31 :       ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: finished");
+     616          31 :       ros::requestShutdown();
+     617          31 :       break;
+     618             :     }
+     619             :   }
+     620             : }
+     621             : 
+     622             : //}
+     623             : 
+     624             : // --------------------------------------------------------------
+     625             : // |                          routines                          |
+     626             : // --------------------------------------------------------------
+     627             : 
+     628             : /* changeState() //{ */
+     629             : 
+     630          51 : void AutomaticStart::changeState(LandingStates_t new_state) {
+     631             : 
+     632          51 :   ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: switching states %s -> %s", state_names[current_state], state_names[new_state]);
+     633             : 
+     634          51 :   switch (new_state) {
+     635             : 
+     636           0 :     case STATE_IDLE: {
+     637             : 
+     638           0 :       break;
+     639             :     }
+     640             : 
+     641          20 :     case STATE_TAKEOFF: {
+     642             : 
+     643          20 :       if (_pre_takeoff_sleep_ > 1.0) {
+     644           0 :         ROS_INFO("[AutomaticStart]: sleeping for %.2f secs before takeoff", _pre_takeoff_sleep_);
+     645           0 :         ros::Duration(_pre_takeoff_sleep_).sleep();
+     646             :       }
+     647             : 
+     648          20 :       bool res = takeoff();
+     649             : 
+     650          20 :       if (!res) {
+     651             : 
+     652           0 :         current_state = STATE_FINISHED;
+     653             : 
+     654           0 :         return;
+     655             :       }
+     656             : 
+     657          20 :       break;
+     658             :     }
+     659             : 
+     660          31 :     case STATE_FINISHED: {
+     661             : 
+     662          31 :       break;
+     663             :     }
+     664             : 
+     665             :     break;
+     666             :   }
+     667             : 
+     668          51 :   current_state = new_state;
+     669             : }
+     670             : 
+     671             : //}
+     672             : 
+     673             : /* takeoff() //{ */
+     674             : 
+     675          20 : bool AutomaticStart::takeoff() {
+     676             : 
+     677          20 :   ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: taking off");
+     678             : 
+     679          40 :   std_srvs::Trigger srv;
+     680             : 
+     681          20 :   bool res = service_client_takeoff_.call(srv);
+     682             : 
+     683          20 :   if (res) {
+     684             : 
+     685          20 :     if (srv.response.success) {
+     686             : 
+     687          20 :       return true;
+     688             : 
+     689             :     } else {
+     690             : 
+     691           0 :       ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: taking off failed: %s", srv.response.message.c_str());
+     692             :     }
+     693             : 
+     694             :   } else {
+     695             : 
+     696           0 :     ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: service call for taking off failed");
+     697             :   }
+     698             : 
+     699           0 :   return false;
+     700             : }
+     701             : 
+     702             : //}
+     703             : 
+     704             : /* validateReference() //{ */
+     705             : 
+     706       10072 : bool AutomaticStart::validateReference() {
+     707             : 
+     708       20144 :   mrs_msgs::ValidateReference srv_out;
+     709             : 
+     710       10072 :   srv_out.request.reference.header.frame_id = _body_frame_name_;
+     711             : 
+     712       10072 :   bool res = service_client_validate_reference_.call(srv_out);
+     713             : 
+     714       10072 :   if (res) {
+     715             : 
+     716       10071 :     if (srv_out.response.success) {
+     717             : 
+     718        9996 :       ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: current position is valid");
+     719        9996 :       return true;
+     720             : 
+     721             :     } else {
+     722             : 
+     723          75 :       ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: current position is not valid (safety area, bumper)!");
+     724          75 :       return false;
+     725             :     }
+     726             : 
+     727             :   } else {
+     728             : 
+     729           1 :     ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: current position could not be validated");
+     730           1 :     return false;
+     731             :   }
+     732             : }
+     733             : 
+     734             : //}
+     735             : 
+     736             : /* toggleControlOutput() //{ */
+     737             : 
+     738          25 : bool AutomaticStart::toggleControlOutput(const bool& value) {
+     739             : 
+     740          25 :   ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: setting control output %s", value ? "ON" : "OFF");
+     741             : 
+     742          50 :   std_srvs::SetBool srv;
+     743          25 :   srv.request.data = value;
+     744             : 
+     745          25 :   bool res = service_client_toggle_control_output_.call(srv);
+     746             : 
+     747          25 :   if (res) {
+     748             : 
+     749          25 :     if (srv.response.success) {
+     750             : 
+     751          25 :       return true;
+     752             : 
+     753             :     } else {
+     754             : 
+     755           0 :       ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: setting of control output failed: %s", srv.response.message.c_str());
+     756             :     }
+     757             : 
+     758             :   } else {
+     759             : 
+     760           0 :     ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: service call for toggling control output failed");
+     761             :   }
+     762             : 
+     763           0 :   return false;
+     764             : }
+     765             : 
+     766             : //}
+     767             : 
+     768             : /* disarm() //{ */
+     769             : 
+     770           2 : bool AutomaticStart::disarm() {
+     771             : 
+     772           2 :   if (!hw_api_connected_) {
+     773             : 
+     774           0 :     ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: cannot disarm, missing HW API status!");
+     775             : 
+     776           0 :     return false;
+     777             :   }
+     778             : 
+     779           2 :   auto [armed, offboard, armed_time, offboard_time] = mrs_lib::get_mutexed(mutex_hw_api_status_, armed_, offboard_, armed_time_, offboard_time_);
+     780             : 
+     781           2 :   if (offboard) {
+     782             : 
+     783           0 :     ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: cannot disarm, already in offboard mode!");
+     784             : 
+     785           0 :     return false;
+     786             :   }
+     787             : 
+     788           2 :   ROS_INFO_THROTTLE(1.0, "[AutomaticStart]: disarming");
+     789             : 
+     790           4 :   std_srvs::SetBool srv;
+     791           2 :   srv.request.data = false;
+     792             : 
+     793           2 :   bool res = service_client_arm_.call(srv);
+     794             : 
+     795           2 :   if (res) {
+     796             : 
+     797           2 :     if (srv.response.success) {
+     798             : 
+     799           2 :       return true;
+     800             : 
+     801             :     } else {
+     802             : 
+     803           0 :       ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: disarming failed");
+     804             :     }
+     805             : 
+     806             :   } else {
+     807             : 
+     808           0 :     ROS_ERROR_THROTTLE(1.0, "[AutomaticStart]: service call for disarming failed");
+     809             :   }
+     810             : 
+     811           0 :   return false;
+     812             : }
+     813             : 
+     814             : //}
+     815             : 
+     816             : /* isGazeboSimulation() //{ */
+     817             : 
+     818       10072 : bool AutomaticStart::isGazeboSimulation(void) {
+     819             : 
+     820       10072 :   if (is_gazebo_simulation_) {
+     821        6745 :     return true;
+     822             :   }
+     823             : 
+     824        6654 :   ros::V_string node_list;
+     825        3327 :   ros::master::getNodes(node_list);
+     826             : 
+     827       49764 :   for (auto& node : node_list) {
+     828       46447 :     if (node.find("mrs_drone_spawner") != std::string::npos) {
+     829          10 :       ROS_INFO("[AutomaticStart]: MRS Gazebo Simulation detected");
+     830          10 :       is_gazebo_simulation_ = true;
+     831          10 :       return true;
+     832             :     }
+     833             :   }
+     834             : 
+     835        3317 :   return false;
+     836             : }
+     837             : 
+     838             : //}
+     839             : 
+     840             : /* topicCheck() //{ */
+     841             : 
+     842       10072 : bool AutomaticStart::topicCheck(void) {
+     843             : 
+     844       10072 :   bool got_topics = true;
+     845             : 
+     846       10072 :   std::stringstream missing_topics;
+     847             : 
+     848       10072 :   if (_topic_check_enabled_) {
+     849             : 
+     850       30291 :     for (int i = 0; i < int(topic_check_topics_.size()); i++) {
+     851             : 
+     852       40363 :       if (topic_check_topics_.at(i).getTime() == ros::Time::UNINITIALIZED ||
+     853       40363 :           (ros::Time::now() - topic_check_topics_.at(i).getTime()) > ros::Duration(_topic_check_timeout_)) {
+     854             : 
+     855          75 :         missing_topics << std::endl << "\t" << topic_check_topics_.at(i).getTopicName();
+     856          75 :         got_topics = false;
+     857             :       }
+     858             :     }
+     859             :   }
+     860             : 
+     861       10072 :   if (!got_topics) {
+     862          75 :     ROS_WARN_STREAM_THROTTLE(1.0, "[AutomaticStart]: missing data on topics: " << missing_topics.str());
+     863             :   }
+     864             : 
+     865       20144 :   return got_topics;
+     866             : }
+     867             : 
+     868             : //}
+     869             : 
+     870             : // | -------- preflight cheks for detecting flyign UAV -------- |
+     871             : 
+     872             : /* preflightCheckSpeed() //{ */
+     873             : 
+     874       10220 : bool AutomaticStart::preflightCheckSpeed(void) {
+     875             : 
+     876       10220 :   if (!_speed_check_enabled_) {
+     877           0 :     return true;
+     878             :   }
+     879             : 
+     880       10220 :   if (!sh_estimation_diag_.hasMsg()) {
+     881           0 :     return false;
+     882             :   }
+     883             : 
+     884       20440 :   auto estimation_diag = sh_estimation_diag_.getMsg();
+     885             : 
+     886       10220 :   double speed = std::hypot(estimation_diag->velocity.linear.x, estimation_diag->velocity.linear.y, estimation_diag->velocity.linear.z);
+     887             : 
+     888       10220 :   if (speed > _speed_check_max_speed_) {
+     889           0 :     speed_check_violated_time_ = ros::Time::now();
+     890           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_);
+     891             :   }
+     892             : 
+     893       10220 :   if (speed_check_violated_time_ != ros::Time::UNINITIALIZED &&
+     894       10220 :       (ros::Time::now() - speed_check_violated_time_) < ros::Duration(_preflight_check_time_window_)) {
+     895           0 :     return false;
+     896             :   } else {
+     897       10220 :     return true;
+     898             :   }
+     899             : }
+     900             : 
+     901             : //}
+     902             : 
+     903             : /* preflighCheckHeight() //{ */
+     904             : 
+     905       10220 : bool AutomaticStart::preflighCheckHeight(void) {
+     906             : 
+     907       10220 :   if (!_height_check_enabled_) {
+     908         246 :     return true;
+     909             :   }
+     910             : 
+     911             :   // | ----------------- is the check possible? ----------------- |
+     912             : 
+     913        9974 :   if (!sh_hw_api_capabilities_.hasMsg()) {
+     914           0 :     return false;
+     915             :   }
+     916             : 
+     917       19948 :   auto capabilities = sh_hw_api_capabilities_.getMsg();
+     918             : 
+     919        9974 :   if (!capabilities->produces_distance_sensor) {
+     920           0 :     return true;
+     921             :   }
+     922             : 
+     923             :   // | -------------------- do we have data? -------------------- |
+     924             : 
+     925        9974 :   if (!sh_distance_sensor_.hasMsg()) {
+     926        1346 :     return true;
+     927             :   }
+     928             : 
+     929        8628 :   double height = sh_distance_sensor_.getMsg()->range;
+     930             : 
+     931        8628 :   if (height > _height_check_max_height_) {
+     932         147 :     height_check_violated_time_ = ros::Time::now();
+     933         147 :     ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: the height (%.2f m) is over the limit (%.2f m)", height, _height_check_max_height_);
+     934             :   }
+     935             : 
+     936        8775 :   if (height_check_violated_time_ != ros::Time::UNINITIALIZED &&
+     937        8775 :       (ros::Time::now() - height_check_violated_time_) < ros::Duration(_preflight_check_time_window_)) {
+     938         147 :     return false;
+     939             :   } else {
+     940        8481 :     return true;
+     941             :   }
+     942             : }
+     943             : 
+     944             : //}
+     945             : 
+     946             : /* preflighCheckGyro() //{ */
+     947             : 
+     948       10220 : bool AutomaticStart::preflighCheckGyro(void) {
+     949             : 
+     950       10220 :   if (!_gyro_check_enabled_) {
+     951           0 :     return true;
+     952             :   }
+     953             : 
+     954             :   // | ----------------- is the check possible? ----------------- |
+     955             : 
+     956       10220 :   if (!sh_hw_api_capabilities_.hasMsg()) {
+     957           0 :     return false;
+     958             :   }
+     959             : 
+     960       20440 :   auto capabilities = sh_hw_api_capabilities_.getMsg();
+     961             : 
+     962       10220 :   if (!capabilities->produces_imu) {
+     963           0 :     return true;
+     964             :   }
+     965             : 
+     966             :   // | -------------------- do we have data? -------------------- |
+     967             : 
+     968       10220 :   if (!sh_imu_.hasMsg()) {
+     969           0 :     return true;
+     970             :   }
+     971             : 
+     972       10220 :   auto gyros = sh_imu_.getMsg()->angular_velocity;
+     973             : 
+     974       10220 :   if (abs(gyros.x) > _gyro_check_max_rate_ || abs(gyros.y) > _gyro_check_max_rate_ || abs(gyros.z) > _gyro_check_max_rate_) {
+     975           1 :     gyro_check_violated_time_ = ros::Time::now();
+     976           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,
+     977             :                       _gyro_check_max_rate_);
+     978             :   }
+     979             : 
+     980       10220 :   if (gyro_check_violated_time_ != ros::Time::UNINITIALIZED && (ros::Time::now() - gyro_check_violated_time_) < ros::Duration(_preflight_check_time_window_)) {
+     981           1 :     return false;
+     982             :   } else {
+     983       10219 :     return true;
+     984             :   }
+     985             : }
+     986             : 
+     987             : //}
+     988             : 
+     989             : }  // namespace automatic_start
+     990             : 
+     991             : }  // namespace mrs_uav_autostart
+     992             : 
+     993             : #include <pluginlib/class_list_macros.h>
+     994          32 : 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..2de9663df5 --- /dev/null +++ b/mrs_uav_autostart/src/automatic_start.cpp.gcov.overview.html @@ -0,0 +1,269 @@ + + + + + + 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 + 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..198e7c82f190c69afddc56e3dcfb21b9a9aca795 GIT binary patch literal 3173 zcmV-r44U(aP)cE0Ro5uT zs6d1~MEl{Ewtwp*F0J>lmfb=s1ptn{T+6PJ@1lUrb<(I1k}!-Jja9%aX!J59iL^8- zin?AJ*=q-iHgaOnND{%PQ7+Z;>jFo*24^pn9D0mxK;tGD82L=Ioe4XxaREaxVrl(f4I_ff0bb;b^hDI(OmikprLY)E0h8h-NjPBbkwE!J0C%X?Lso-zDf*fuI ze>`vf7$LT7Z4JPB+FW3)-2p0FceCLzLh(4X;5#$o!Pc^@;j21~S#3ZE+X4(MGyo&3 zYxqQLeJ&P_JgyxL5pYI@8$>Ci!a2vD)40jd$O!?4;x+6Xxyk=oq9~anK*IIwwvsSC z#&smNR89C?`tYg~4>|?^eu8C#DECf?qg=-<+{NoH;cZ5aeqp{G9@oN&T2b9bBR>HxOfEQU0*j+jUr z|1B;W$wDFL_uK!WWQ zBi#mAQcpfBNKB3(T#q{de-)<0lLi)6i;B<8>i(L&O0oJ1Lo{wt=5w)dNs^;#WVq$& zeH~+-vk7wT^&-o4{dg5z*Vrz(o^GcUTu(Bcb6xi-IoAZnoa=t7GK_(1=S)PCVKR_F z>Y-mlFak92!?2;J;v+Ja)WRepZLAo%?0R$GSqt&x{xMmb^Oqk}50@R2hl`8dW#gKG%m>%Y3)@kv^6_0~#nS8CtSju_oJ^DQAo^F1U-ReX^)7P~M;b=Mdg z)%B6#KL8*fW3+Ik>n847l`Xryt__H;ws@U?=7}<&{2nYzFXjIK)?*2MpdA}-0pI} zKa=%!{{DJCUcc&|$2H;RBLYrjj}UG8e#D$xPjXG%^8?=4cVmO+gxMZb)q%c5DI0U3h?e zjG6>wYlaIb-%j<+R82zlG9y3KkOX3iWy%J?z%0>#8P9sEZ%fWG zU_IE%0D0HpYevrVp628zM_TVGY$JR%5f!#DYA6u78nf3-8lk3D&MX9E7Ct(*zNkh@ ztSthj|L&F*qX8fj;{c6PPfMUN$mO9^#|^#K1~ZO&DktpQdO61lLkE71Ga7ctFmCOY zO=`M7RoJOJI0S@24hQcD?EH0#WR2;3O+P0S%4SSTCSf;LVMaae=5?b8$B?v%Zq2m% zJ#WvX;*bIb=cHrw(yDwgnQ><(-u1%qb&cIZGg8NO-(fed4S2L3WV`gtc=c3{O*k|5 zKUANKx0Zo3(|j6`x%J&vd>@ZS$@c}wi*Xm6879-PSU{)j{Kn!1=l|+FTp;VJ=eSn5 zKGFS;QZ5j^`FS{deaGc&cAAAW3i{&n>x|+kTJjyvo~!G&c2tP>!ebt6NSUA6XKMYfbZ@>(+pN^(%tMKnD^^iMY_G0Peq^W z8o0b1kgu78b?NGJ&8ms7@0ue048$IZOeBZV80O`;fDq#Z*8rop>?0nXaCW*nknnPU zcoUnWvEEwH@!)o1R88aQq_IMzsfufN_4E0WHbRLhScMZktE`=Tp177Eusc5t@frLla7g}IW{m8x@%Ie2^08>vi)lq@t1XPIt)*wVesc%1@GIXi zdo9<3tc@V3%<=WLU2wBC(eOO`=F>Y)0`V{cv32G@%Y-PeY0a^0t5f3OdiA;U&tU^j7-j)pFg?qBx7P*Zq2c6{+<(yAp5W9@9yA+O1&(;CK{IRq3}CFk zd!Zj=(Zt1EYUk`svy##|p^>!gBrv-4LySi8E0>=*PfLVP7lB${3zsDDUtZA z(ZkhCzQHK+C;gznqj{$yOg{ElPy5kJ11Nu2_-kKL#yjfv==;pOu6xPP{M^D%znu7? zpI8cqj!FCkM_T5NiG+uBUALY$hPdn<2T8>q+O&zQR=B4I&bRGcAf8l(lRU%ZE7fnTogGT`QNkLWSMBhDiPG2YfFJarGZeK!&*RT1QoV z|0D9UjVoUZ?ub+Pr@s& + + + + + + 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:29333587.5 %
Date:2024-12-17 22:35:29Functions: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
automatic_start.cpp +
87.5%87.5%
+ 		87.5 %293 / 335100.0 %22 / 22
<unnamed>87.5 %293 / 335100.0 %22 / 22
+
+
+ + + + +
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..7a786b652f --- /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:29333587.5 %
Date:2024-12-17 22:35:29Functions: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
automatic_start.cpp +
87.5%87.5%
+ 		87.5 %293 / 335100.0 %22 / 22
<unnamed>87.5 %293 / 335100.0 %22 / 22
+
+
+ + + + +
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..ac0a2c5815 --- /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:29333587.5 %
Date:2024-12-17 22:35:29Functions: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
automatic_start.cpp +
87.5%87.5%
+ 		87.5 %293 / 335100.0 %22 / 22
<unnamed>87.5 %293 / 335100.0 %22 / 22
+
+
+ + + + +
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..50148c8983 --- /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:29333587.5 %
Date:2024-12-17 22:35:29Functions: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
automatic_start.cpp +
87.5%87.5%
+ 		87.5 %293 / 335100.0 %22 / 22
+
+
+ + + + +
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..088abfd5e2 --- /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:29333587.5 %
Date:2024-12-17 22:35:29Functions: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
automatic_start.cpp +
87.5%87.5%
+ 		87.5 %293 / 335100.0 %22 / 22
+
+
+ + + + +
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..c1b5361496 --- /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:29333587.5 %
Date:2024-12-17 22:35:29Functions: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
automatic_start.cpp +
87.5%87.5%
+ 		87.5 %293 / 335100.0 %22 / 22
+
+
+ + + + +
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..ec61cc9ab9 --- /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-12-17 22:35:29Functions: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::HwApiAttitudeRateCmd_<std::allocator<void> > const&)41
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)52
+
+
+ + + +
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..2c97202d6c --- /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-12-17 22:35:29Functions: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&)52
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&)41
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..4ab7b9edbb --- /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-12-17 22:35:29Functions: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          52 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeCmd& msg) {
+      77          52 :     return msg.throttle;
+      78             :   }
+      79          41 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeRateCmd& msg) {
+      80          41 :     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-12-17 22:35:29Functions: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..1fa4e6994a --- /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-12-17 22:35:29Functions: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..a9f916e3c2 --- /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-12-17 22:35:29Functions: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..d734ec6202 --- /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-12-17 22:35:29Functions: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..37a167b6b4 --- /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-12-17 22:35:29Functions: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..e032a50010 --- /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-12-17 22:35:29Functions: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..15c666204d --- /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-12-17 22:35:29Functions:55100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::PIDController::reset()1308
mrs_uav_controllers::PIDController::setParams(double const&, double const&, double const&, double const&, double const&)1308
mrs_uav_controllers::PIDController::PIDController()1308
mrs_uav_controllers::PIDController::setSaturation(double)15266
mrs_uav_controllers::PIDController::update(double const&, double const&)15266
+
+
+ + + +
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..1b11eb7305 --- /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-12-17 22:35:29Functions:55100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::PIDController::setSaturation(double)15266
mrs_uav_controllers::PIDController::reset()1308
mrs_uav_controllers::PIDController::update(double const&, double const&)15266
mrs_uav_controllers::PIDController::setParams(double const&, double const&, double const&, double const&, double const&)1308
mrs_uav_controllers::PIDController::PIDController()1308
+
+
+ + + +
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..255c75e2d3 --- /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-12-17 22:35:29Functions: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        1308 : PIDController::PIDController() {
+      43             : 
+      44        1308 :   this->reset();
+      45        1308 : }
+      46             : 
+      47        1308 : void PIDController::setParams(const double &kp, const double &kd, const double &ki, const double &saturation, const double &antiwindup) {
+      48             : 
+      49        1308 :   this->_kp_       = kp;
+      50        1308 :   this->_kd_       = kd;
+      51        1308 :   this->_ki_       = ki;
+      52        1308 :   this->saturation = saturation;
+      53        1308 :   this->antiwindup = antiwindup;
+      54        1308 : }
+      55             : 
+      56       15266 : void PIDController::setSaturation(const double saturation) {
+      57             : 
+      58       15266 :   this->saturation = saturation;
+      59       15266 : }
+      60             : 
+      61        1308 : void PIDController::reset(void) {
+      62             : 
+      63        1308 :   this->last_error_ = 0;
+      64        1308 :   this->integral_   = 0;
+      65        1308 : }
+      66             : 
+      67       15266 : double PIDController::update(const double &error, const double &dt) {
+      68             : 
+      69             :   // calculate the control error difference
+      70       15266 :   double difference = (error - last_error_) / dt;
+      71       15266 :   last_error_       = error;
+      72             : 
+      73       15266 :   double p_component = _kp_ * error;       // proportional feedback
+      74       15266 :   double d_component = _kd_ * difference;  // derivative feedback
+      75       15266 :   double i_component = _ki_ * integral_;   // derivative feedback
+      76             : 
+      77       15266 :   double sum = p_component + d_component + i_component;
+      78             : 
+      79       15266 :   if (saturation > 0) {
+      80       15266 :     if (sum >= saturation) {
+      81         114 :       sum = saturation;
+      82       15152 :     } else if (sum <= -saturation) {
+      83         173 :       sum = -saturation;
+      84             :     }
+      85             :   }
+      86             : 
+      87       15266 :   if (antiwindup > 0) {
+      88       15266 :     if (std::abs(sum) < antiwindup) {
+      89             :       // add to the integral
+      90       14904 :       integral_ += error * dt;
+      91             :     }
+      92             :   }
+      93             : 
+      94             :   // return the summ of the components
+      95       15266 :   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..d2cc0f0797 --- /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-12-17 22:35:29Functions: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&)258
mrs_uav_controllers::common::actuatorMixer(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&)3998
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&)7952
mrs_uav_controllers::common::getHighestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)10205
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&)94534
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&)94534
mrs_uav_controllers::common::orientationError(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&)96373
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&)96373
mrs_uav_controllers::common::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)101142
+
+
+ + + +
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..0718cc6e00 --- /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-12-17 22:35:29Functions: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&)94534
mrs_uav_controllers::common::actuatorMixer(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&)3998
mrs_uav_controllers::common::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)101142
mrs_uav_controllers::common::extractThrottle(mrs_uav_managers::Controller::ControlOutput const&)258
mrs_uav_controllers::common::getHighestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)10205
mrs_uav_controllers::common::orientationError(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&)96373
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&)96373
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&)94534
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&)7952
+
+
+ + + +
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..92b635fd1a --- /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-12-17 22:35:29Functions: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       96373 : Eigen::Vector3d orientationError(const Eigen::Matrix3d& R, const Eigen::Matrix3d& Rd) {
+      12             : 
+      13             :   // orientation error
+      14       96373 :   Eigen::Matrix3d R_error = 0.5 * (Rd.transpose() * R - R.transpose() * Rd);
+      15             : 
+      16             :   // vectorize the orientation error
+      17             :   // clang-format off
+      18       96373 :     Eigen::Vector3d R_error_vec;
+      19       96373 :     R_error_vec << (R_error(1, 2) - R_error(2, 1)) / 2.0,
+      20       96373 :                    (R_error(2, 0) - R_error(0, 2)) / 2.0,
+      21       96373 :                    (R_error(0, 1) - R_error(1, 0)) / 2.0;
+      22             :   // clang-format on
+      23             : 
+      24      192746 :   return R_error_vec;
+      25             : }
+      26             : 
+      27             : //}
+      28             : 
+      29             : /* sanitizeDesiredForce() //{ */
+      30             : 
+      31       94534 : 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       94534 :   double theta = acos(input(2));
+      36       94534 :   double phi   = atan2(input(1), input(0));
+      37             : 
+      38             :   // check for the failsafe limit
+      39       94534 :   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       94534 :   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       94534 :   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       94534 :   Eigen::Vector3d output(sin(theta) * cos(phi), sin(theta) * sin(phi), cos(theta));
+      63             : 
+      64       94534 :   return {output};
+      65             : }
+      66             : 
+      67             : //}
+      68             : 
+      69             : /* so3transform() //{ */
+      70             : 
+      71       94534 : Eigen::Matrix3d so3transform(const Eigen::Vector3d& body_z, const ::Eigen::Vector3d& heading, const bool& preserve_heading) {
+      72             : 
+      73       94534 :   Eigen::Vector3d body_z_normed = body_z.normalized();
+      74             : 
+      75       94534 :   Eigen::Matrix3d Rd;
+      76             : 
+      77       94534 :   if (preserve_heading) {
+      78             : 
+      79       23978 :     ROS_DEBUG_THROTTLE(1.0, "[SO3Transform]: using Baca's method");
+      80             : 
+      81             :     // | ------------------------- body z ------------------------- |
+      82       23978 :     Rd.col(2) = body_z_normed;
+      83             : 
+      84             :     // | ------------------------- body x ------------------------- |
+      85             : 
+      86             :     // construct the oblique projection
+      87       23978 :     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       47956 :     Eigen::MatrixXd A = Eigen::MatrixXd(3, 2);
+      91       23978 :     A.col(0)          = projector_body_z_compl.col(0);
+      92       23978 :     A.col(1)          = projector_body_z_compl.col(1);
+      93             : 
+      94             :     // create the basis of the projection null-space complement
+      95       47956 :     Eigen::MatrixXd B = Eigen::MatrixXd(3, 2);
+      96       23978 :     B.col(0)          = Eigen::Vector3d(1, 0, 0);
+      97       23978 :     B.col(1)          = Eigen::Vector3d(0, 1, 0);
+      98             : 
+      99             :     // oblique projector to <range_basis>
+     100       47956 :     Eigen::MatrixXd Bt_A               = B.transpose() * A;
+     101       47956 :     Eigen::MatrixXd Bt_A_pseudoinverse = ((Bt_A.transpose() * Bt_A).inverse()) * Bt_A.transpose();
+     102       23978 :     Eigen::MatrixXd oblique_projector  = A * Bt_A_pseudoinverse * B.transpose();
+     103             : 
+     104       23978 :     Rd.col(0) = oblique_projector * heading;
+     105       23978 :     Rd.col(0).normalize();
+     106             : 
+     107             :     // | ------------------------- body y ------------------------- |
+     108             : 
+     109       23978 :     Rd.col(1) = Rd.col(2).cross(Rd.col(0));
+     110       23978 :     Rd.col(1).normalize();
+     111             : 
+     112             :   } else {
+     113             : 
+     114       70556 :     ROS_DEBUG_THROTTLE(1.0, "[SO3Transform]: using Lee's method");
+     115             : 
+     116       70556 :     Rd.col(2) = body_z_normed;
+     117       70556 :     Rd.col(1) = Rd.col(2).cross(heading);
+     118       70556 :     Rd.col(1).normalize();
+     119       70556 :     Rd.col(0) = Rd.col(1).cross(Rd.col(2));
+     120       70556 :     Rd.col(0).normalize();
+     121             :   }
+     122             : 
+     123      189068 :   return Rd;
+     124             : }
+     125             : 
+     126             : //}
+     127             : 
+     128             : /* getLowestOutput() //{ */
+     129             : 
+     130      101142 : std::optional<CONTROL_OUTPUT> getLowestOuput(const mrs_uav_managers::control_manager::ControlOutputModalities_t& outputs) {
+     131             : 
+     132      101142 :   if (outputs.actuators) {
+     133        2604 :     return ACTUATORS_CMD;
+     134             :   }
+     135             : 
+     136       98538 :   if (outputs.control_group) {
+     137        2561 :     return CONTROL_GROUP;
+     138             :   }
+     139             : 
+     140       95977 :   if (outputs.attitude_rate) {
+     141       87027 :     return ATTITUDE_RATE;
+     142             :   }
+     143             : 
+     144        8950 :   if (outputs.attitude) {
+     145        2342 :     return ATTITUDE;
+     146             :   }
+     147             : 
+     148        6608 :   if (outputs.acceleration_hdg_rate) {
+     149        1131 :     return ACCELERATION_HDG_RATE;
+     150             :   }
+     151             : 
+     152        5477 :   if (outputs.acceleration_hdg) {
+     153        1080 :     return ACCELERATION_HDG;
+     154             :   }
+     155             : 
+     156        4397 :   if (outputs.velocity_hdg_rate) {
+     157        2459 :     return VELOCITY_HDG_RATE;
+     158             :   }
+     159             : 
+     160        1938 :   if (outputs.velocity_hdg) {
+     161        1433 :     return VELOCITY_HDG;
+     162             :   }
+     163             : 
+     164         505 :   if (outputs.position) {
+     165         505 :     return POSITION;
+     166             :   }
+     167             : 
+     168           0 :   return {};
+     169             : }
+     170             : 
+     171             : //}
+     172             : 
+     173             : /* getHighestOutput() //{ */
+     174             : 
+     175       10205 : std::optional<CONTROL_OUTPUT> getHighestOuput(const mrs_uav_managers::control_manager::ControlOutputModalities_t& outputs) {
+     176             : 
+     177       10205 :   if (outputs.position) {
+     178           3 :     return POSITION;
+     179             :   }
+     180             : 
+     181       10202 :   if (outputs.velocity_hdg) {
+     182           6 :     return VELOCITY_HDG;
+     183             :   }
+     184             : 
+     185       10196 :   if (outputs.velocity_hdg_rate) {
+     186          15 :     return VELOCITY_HDG_RATE;
+     187             :   }
+     188             : 
+     189       10181 :   if (outputs.acceleration_hdg) {
+     190           6 :     return ACCELERATION_HDG;
+     191             :   }
+     192             : 
+     193       10175 :   if (outputs.acceleration_hdg_rate) {
+     194           6 :     return ACCELERATION_HDG_RATE;
+     195             :   }
+     196             : 
+     197       10169 :   if (outputs.attitude) {
+     198        5907 :     return ATTITUDE;
+     199             :   }
+     200             : 
+     201        4262 :   if (outputs.attitude_rate) {
+     202        1429 :     return ATTITUDE_RATE;
+     203             :   }
+     204             : 
+     205        2833 :   if (outputs.control_group) {
+     206        1415 :     return CONTROL_GROUP;
+     207             :   }
+     208             : 
+     209        1418 :   if (outputs.actuators) {
+     210        1418 :     return ACTUATORS_CMD;
+     211             :   }
+     212             : 
+     213           0 :   return {};
+     214             : }
+     215             : 
+     216             : //}
+     217             : 
+     218             : /* extractThrottle() //{ */
+     219             : 
+     220         258 : std::optional<double> extractThrottle(const mrs_uav_managers::Controller::ControlOutput& control_output) {
+     221             : 
+     222         258 :   if (!control_output.control_output) {
+     223         139 :     return {};
+     224             :   }
+     225             : 
+     226         238 :   return std::visit(HwApiCmdExtractThrottleVisitor(), control_output.control_output.value());
+     227             : }
+     228             : 
+     229             : //}
+     230             : 
+     231             : /* attitudeController() //{ */
+     232             : 
+     233       96373 : 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       96373 :   Eigen::Matrix3d R  = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+     238       96373 :   Eigen::Matrix3d Rd = mrs_lib::AttitudeConverter(reference.orientation);
+     239             : 
+     240             :   // calculate the orientation error
+     241       96373 :   Eigen::Vector3d E = common::orientationError(R, Rd);
+     242             : 
+     243       96373 :   Eigen::Vector3d rate_feedback = gains.array() * E.array() + ff_rate.array();
+     244             : 
+     245             :   // | ----------- parasitic heading rate compensation ---------- |
+     246             : 
+     247       96373 :   if (parasitic_heading_rate_compensation) {
+     248             : 
+     249       22779 :     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       22779 :     Eigen::Vector3d rp_heading_rate_compensation(0, 0, 0);
+     253             : 
+     254       22779 :     Eigen::Vector3d q_feedback_yawless = rate_feedback;
+     255       22779 :     q_feedback_yawless(2)              = 0;  // nullyfy the effect of the original yaw feedback
+     256             : 
+     257       22779 :     double parasitic_heading_rate = 0;
+     258             : 
+     259             :     try {
+     260       22779 :       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       22779 :       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       22779 :     rate_feedback += rp_heading_rate_compensation;
+     274             :   }
+     275             : 
+     276             :   // | --------------- saturate the attitude rate --------------- |
+     277             : 
+     278       96373 :   if (rate_feedback(0) > rate_saturation(0)) {
+     279           0 :     rate_feedback(0) = rate_saturation(0);
+     280       96373 :   } else if (rate_feedback(0) < -rate_saturation(0)) {
+     281           0 :     rate_feedback(0) = -rate_saturation(0);
+     282             :   }
+     283             : 
+     284       96373 :   if (rate_feedback(1) > rate_saturation(1)) {
+     285           0 :     rate_feedback(1) = rate_saturation(1);
+     286       96373 :   } else if (rate_feedback(1) < -rate_saturation(1)) {
+     287           0 :     rate_feedback(1) = -rate_saturation(1);
+     288             :   }
+     289             : 
+     290       96373 :   if (rate_feedback(2) > rate_saturation(2)) {
+     291           0 :     rate_feedback(2) = rate_saturation(2);
+     292       96373 :   } 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       96373 :   mrs_msgs::HwApiAttitudeRateCmd cmd;
+     299             : 
+     300       96373 :   cmd.stamp = ros::Time::now();
+     301             : 
+     302       96373 :   cmd.body_rate.x = rate_feedback(0);
+     303       96373 :   cmd.body_rate.y = rate_feedback(1);
+     304       96373 :   cmd.body_rate.z = rate_feedback(2);
+     305             : 
+     306       96373 :   cmd.throttle = reference.throttle;
+     307             : 
+     308      192746 :   return cmd;
+     309             : }
+     310             : 
+     311             : //}
+     312             : 
+     313             : /* attitudeRateController() //{ */
+     314             : 
+     315        7952 : std::optional<mrs_msgs::HwApiControlGroupCmd> attitudeRateController(const mrs_msgs::UavState& uav_state, const mrs_msgs::HwApiAttitudeRateCmd& reference,
+     316             :                                                                      const Eigen::Vector3d& gains) {
+     317             : 
+     318        7952 :   Eigen::Vector3d des_rate(reference.body_rate.x, reference.body_rate.y, reference.body_rate.z);
+     319        7952 :   Eigen::Vector3d cur_rate(uav_state.velocity.angular.x, uav_state.velocity.angular.y, uav_state.velocity.angular.z);
+     320             : 
+     321        7952 :   Eigen::Vector3d ctrl_group_action = (des_rate - cur_rate).array() * gains.array();
+     322             : 
+     323        7952 :   mrs_msgs::HwApiControlGroupCmd cmd;
+     324             : 
+     325        7952 :   cmd.stamp = ros::Time::now();
+     326             : 
+     327        7952 :   cmd.throttle = reference.throttle;
+     328             : 
+     329        7952 :   cmd.roll  = ctrl_group_action(0);
+     330        7952 :   cmd.pitch = ctrl_group_action(1);
+     331        7952 :   cmd.yaw   = ctrl_group_action(2);
+     332             : 
+     333       15904 :   return {cmd};
+     334             : }
+     335             : 
+     336             : //}
+     337             : 
+     338             : /* actuatorMixer() //{ */
+     339             : 
+     340        3998 : mrs_msgs::HwApiActuatorCmd actuatorMixer(const mrs_msgs::HwApiControlGroupCmd& ctrl_group_cmd, const Eigen::MatrixXd& mixer) {
+     341             : 
+     342        3998 :   Eigen::Vector4d ctrl_group(ctrl_group_cmd.roll, ctrl_group_cmd.pitch, ctrl_group_cmd.yaw, ctrl_group_cmd.throttle);
+     343             : 
+     344        7996 :   Eigen::VectorXd motors = mixer * ctrl_group;
+     345             : 
+     346        3998 :   double min = motors.minCoeff();
+     347             : 
+     348        3998 :   if (min < 0.0) {
+     349           0 :     motors.array() += abs(min);
+     350             :   }
+     351             : 
+     352        3998 :   double max = motors.maxCoeff();
+     353             : 
+     354        3998 :   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        3998 :   mrs_msgs::HwApiActuatorCmd actuator_msg;
+     373             : 
+     374        3998 :   actuator_msg.stamp = ros::Time::now();
+     375             : 
+     376       19990 :   for (int i = 0; i < motors.size(); i++) {
+     377       15992 :     actuator_msg.motors.push_back(motors(i));
+     378             :   }
+     379             : 
+     380        7996 :   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-12-17 22:35:29Functions: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&)7
mrs_uav_controllers::failsafe_controller::FailsafeController::activate(mrs_uav_managers::Controller::ControlOutput const&)7
mrs_uav_controllers::failsafe_controller::FailsafeController::deactivate()34
(anonymous namespace)::ProxyExec0::ProxyExec0()109
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>)109
mrs_uav_controllers::failsafe_controller::FailsafeController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)431
mrs_uav_controllers::failsafe_controller::FailsafeController::getStatus()970
mrs_uav_controllers::failsafe_controller::FailsafeController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)9718
mrs_uav_controllers::failsafe_controller::FailsafeController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)142107
+
+
+ + + +
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..c96ed77273 --- /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-12-17 22:35:29Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()109
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>)109
mrs_uav_controllers::failsafe_controller::FailsafeController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)9718
mrs_uav_controllers::failsafe_controller::FailsafeController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)431
mrs_uav_controllers::failsafe_controller::FailsafeController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)142107
mrs_uav_controllers::failsafe_controller::FailsafeController::getHeadingSafely(geometry_msgs::Quaternion_<std::allocator<void> > const&)7
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&)7
mrs_uav_controllers::failsafe_controller::FailsafeController::getStatus()970
+
+
+ + + +
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..3f7903fc2a --- /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-12-17 22:35:29Functions: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         109 : 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         109 :   nh_ = nh;
+     117             : 
+     118         109 :   common_handlers_  = common_handlers;
+     119         109 :   private_handlers_ = private_handlers;
+     120             : 
+     121         109 :   _uav_mass_ = common_handlers->getMass();
+     122             : 
+     123         109 :   ros::Time::waitForValid();
+     124             : 
+     125             :   // | ---------- loading params using the parent's nh ---------- |
+     126             : 
+     127         218 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     128             : 
+     129         109 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     130             : 
+     131         109 :   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         109 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/failsafe_controller.yaml");
+     139         109 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/failsafe_controller.yaml");
+     140             : 
+     141         218 :   const std::string yaml_namespace = "mrs_uav_controllers/failsafe_controller/";
+     142             : 
+     143         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "throttle_output/throttle_decrease_rate", _throttle_decrease_rate_);
+     144         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "throttle_output/initial_throttle_percentage", _initial_throttle_percentage_);
+     145             : 
+     146         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "attitude_controller/gains/kp", _kq_);
+     147             : 
+     148         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "rate_controller/gains/kp", _kw_);
+     149             : 
+     150         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "velocity_output/descend_speed", _descend_speed_);
+     151         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "acceleration_output/descend_acceleration", _descend_acceleration_);
+     152             : 
+     153         109 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     154           0 :     ROS_ERROR("[FailsafeController]: Could not load all parameters!");
+     155           0 :     return false;
+     156             :   }
+     157             : 
+     158         109 :   _descend_speed_        = std::abs(_descend_speed_);
+     159         109 :   _descend_acceleration_ = std::abs(_descend_acceleration_);
+     160             : 
+     161         109 :   uav_mass_difference_ = 0;
+     162             : 
+     163             :   // | ----------------------- subscribers ---------------------- |
+     164             : 
+     165         109 :   mrs_lib::SubscribeHandlerOptions shopts;
+     166         109 :   shopts.nh                 = nh_;
+     167         109 :   shopts.node_name          = "FailsafeController";
+     168         109 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     169         109 :   shopts.threadsafe         = true;
+     170         109 :   shopts.autostart          = true;
+     171         109 :   shopts.queue_size         = 10;
+     172         109 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     173             : 
+     174         109 :   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         109 :   hover_throttle_ = mrs_lib::quadratic_throttle_model::forceToThrottle(common_handlers_->throttle_model, _uav_mass_ * common_handlers_->g);
+     179             : 
+     180             :   // | ------------------------ profiler ------------------------ |
+     181             : 
+     182         109 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "FailsafeController", _profiler_enabled_);
+     183             : 
+     184             :   // | ----------------------- finish init ---------------------- |
+     185             : 
+     186         109 :   ROS_INFO("[FailsafeController]: initialized");
+     187             : 
+     188         109 :   is_initialized_ = true;
+     189             : 
+     190         109 :   return true;
+     191             : }
+     192             : 
+     193             : //}
+     194             : 
+     195             : /* activate() //{ */
+     196             : 
+     197           7 : bool FailsafeController::activate(const ControlOutput &last_control_output) {
+     198             : 
+     199          14 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     200             : 
+     201           7 :   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           7 :     if (sh_hw_api_orientation_.getMsg()) {
+     212             : 
+     213          14 :       auto hw_api_orientation = sh_hw_api_orientation_.getMsg();
+     214             : 
+     215           7 :       heading_setpoint_ = getHeadingSafely(hw_api_orientation->quaternion);
+     216             : 
+     217           7 :       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           7 :     activation_control_output_ = last_control_output;
+     227             : 
+     228           7 :     if (last_control_output.diagnostics.mass_estimator) {
+     229           7 :       uav_mass_difference_ = last_control_output.diagnostics.mass_difference;
+     230             :     } else {
+     231           0 :       uav_mass_difference_ = 0;
+     232             :     }
+     233             : 
+     234           7 :     activation_control_output_.diagnostics.controller_enforcing_constraints = false;
+     235             : 
+     236           7 :     hover_throttle_ = _initial_throttle_percentage_ * mrs_lib::quadratic_throttle_model::forceToThrottle(
+     237           7 :                                                           common_handlers_->throttle_model, (_uav_mass_ + uav_mass_difference_) * common_handlers_->g);
+     238             : 
+     239           7 :     ROS_INFO("[FailsafeController]: activated with uav_mass_difference %.2f kg.", uav_mass_difference_);
+     240             :   }
+     241             : 
+     242           7 :   first_iteration_ = true;
+     243             : 
+     244           7 :   is_active_ = true;
+     245             : 
+     246           7 :   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      142107 : void FailsafeController::updateInactive(const mrs_msgs::UavState &uav_state, [[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &tracker_command) {
+     267             : 
+     268      142107 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     269             : 
+     270      142107 :   last_update_time_ = ros::Time::now();
+     271             : 
+     272      142107 :   first_iteration_ = false;
+     273      142107 : }
+     274             : 
+     275             : //}
+     276             : 
+     277             : /* //{ updateWhenAcctive() */
+     278             : 
+     279        9718 : FailsafeController::ControlOutput FailsafeController::updateActive(const mrs_msgs::UavState &                       uav_state,
+     280             :                                                                    [[maybe_unused]] const mrs_msgs::TrackerCommand &tracker_command) {
+     281             : 
+     282       29154 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     283       29154 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("FailsafeController::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     284             : 
+     285             :   {
+     286       19436 :     std::scoped_lock lock(mutex_uav_state_);
+     287             : 
+     288        9718 :     uav_state_ = uav_state;
+     289             :   }
+     290             : 
+     291        9718 :   if (!is_active_) {
+     292           0 :     return ControlOutput();
+     293             :   }
+     294             : 
+     295        9718 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     296             : 
+     297             :   // | -------------------- calculate the dt -------------------- |
+     298             : 
+     299             :   double dt;
+     300             : 
+     301        9718 :   if (first_iteration_) {
+     302           7 :     dt               = 0.01;
+     303           7 :     first_iteration_ = false;
+     304             :   } else {
+     305        9711 :     dt = (ros::Time::now() - last_update_time_).toSec();
+     306             :   }
+     307             : 
+     308        9718 :   last_update_time_ = ros::Time::now();
+     309             : 
+     310        9718 :   hover_throttle_ -= _throttle_decrease_rate_ * dt;
+     311             : 
+     312        9718 :   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        9718 :   } else if (hover_throttle_ > 1.0) {
+     316           0 :     hover_throttle_ = 1.0;
+     317        9718 :   } else if (hover_throttle_ < 0.0) {
+     318           0 :     hover_throttle_ = 0.0;
+     319             :   }
+     320             : 
+     321             :   // | --------------- prepare the control output --------------- |
+     322             : 
+     323       19436 :   FailsafeController::ControlOutput control_output;
+     324             : 
+     325        9718 :   control_output.diagnostics.controller = "FailsafeController";
+     326             : 
+     327        9718 :   auto highest_modality = common::getHighestOuput(common_handlers_->control_output_modalities);
+     328             : 
+     329        9718 :   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        9718 :   control_output.diagnostics.controller = "FailsafeController";
+     335             : 
+     336             :   // --------------------------------------------------------------
+     337             :   // |                       position output                      |
+     338             :   // --------------------------------------------------------------
+     339             : 
+     340        9718 :   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        9718 :   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        9718 :   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        9718 :   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        9718 :   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        9718 :   mrs_msgs::HwApiAttitudeCmd attitude_cmd;
+     422             : 
+     423        9718 :   attitude_cmd.stamp       = ros::Time::now();
+     424        9718 :   attitude_cmd.orientation = mrs_lib::AttitudeConverter(0, 0, heading_setpoint_);
+     425        9718 :   attitude_cmd.throttle    = hover_throttle_;
+     426             : 
+     427        9718 :   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        4181 :   Eigen::Vector3d attitude_rate_saturation(constraints.roll_rate, constraints.pitch_rate, constraints.yaw_rate);
+     438        4181 :   Eigen::Vector3d rate_ff(0, 0, 0);
+     439        4181 :   Eigen::Vector3d Kq(_kq_, _kq_, _kq_);
+     440             : 
+     441        4181 :   auto attitude_rate_command = common::attitudeController(uav_state, attitude_cmd, rate_ff, attitude_rate_saturation, Kq, false);
+     442             : 
+     443        4181 :   if (highest_modality.value() == common::ATTITUDE_RATE) {
+     444        1394 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning attitude rate output");
+     445        1394 :     control_output.control_output = attitude_rate_command;
+     446        1394 :     return control_output;
+     447             :   }
+     448             : 
+     449             :   // --------------------------------------------------------------
+     450             :   // |                    attitude rate control                   |
+     451             :   // --------------------------------------------------------------
+     452             : 
+     453        2787 :   Eigen::Vector3d Kw = common_handlers_->detailed_model_params->inertia.diagonal() * _kw_;
+     454             : 
+     455        2787 :   auto control_group_command = common::attitudeRateController(uav_state, attitude_rate_command.value(), Kw);
+     456             : 
+     457        2787 :   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        2788 :   mrs_msgs::HwApiActuatorCmd actuator_cmd = common::actuatorMixer(control_group_command.value(), common_handlers_->detailed_model_params->control_group_mixer);
+     468             : 
+     469        1394 :   ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning actuators output");
+     470        1394 :   control_output.control_output = actuator_cmd;
+     471        1394 :   return control_output;
+     472             : }
+     473             : 
+     474             : //}
+     475             : 
+     476             : /* getStatus() //{ */
+     477             : 
+     478         970 : const mrs_msgs::ControllerStatus FailsafeController::getStatus() {
+     479             : 
+     480         970 :   mrs_msgs::ControllerStatus controller_status;
+     481             : 
+     482         970 :   controller_status.active = is_active_;
+     483             : 
+     484         970 :   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         431 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr FailsafeController::setConstraints([
+     506             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &constraints) {
+     507             : 
+     508         431 :   if (!is_initialized_) {
+     509           0 :     return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+     510             :   }
+     511             : 
+     512         431 :   mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_);
+     513             : 
+     514         431 :   ROS_INFO("[FailsafeController]: updating constraints");
+     515             : 
+     516         862 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     517         431 :   res.success = true;
+     518         431 :   res.message = "constraints updated";
+     519             : 
+     520         431 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     521             : }
+     522             : 
+     523             : //}
+     524             : 
+     525             : /* getHeadingSafely() //{ */
+     526             : 
+     527           7 : double FailsafeController::getHeadingSafely(const geometry_msgs::Quaternion &quaternion) {
+     528             : 
+     529             :   try {
+     530           7 :     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         109 : 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..101a65b806 --- /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-12-17 22:35:29Functions: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..97bbc9900b --- /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-12-17 22:35:29Functions: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..a5762f516e --- /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-12-17 22:35:29Functions: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..834f4d9e90 --- /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-12-17 22:35:29Functions: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..f3027d97a5 --- /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-12-17 22:35:29Functions: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..41f59cbdf0 --- /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-12-17 22:35:29Functions: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..07b2613337 --- /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-12-17 22:35:29Functions: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&)15
mrs_uav_controllers::midair_activation_controller::MidairActivationController::deactivate()103
(anonymous namespace)::ProxyExec0::ProxyExec0()109
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>)109
mrs_uav_controllers::midair_activation_controller::MidairActivationController::getStatus()145
mrs_uav_controllers::midair_activation_controller::MidairActivationController::activate(mrs_uav_managers::Controller::ControlOutput const&)163
mrs_uav_controllers::midair_activation_controller::MidairActivationController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)431
mrs_uav_controllers::midair_activation_controller::MidairActivationController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)487
mrs_uav_controllers::midair_activation_controller::MidairActivationController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)151338
+
+
+ + + +
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..9ab6315ab0 --- /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-12-17 22:35:29Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()109
mrs_uav_controllers::midair_activation_controller::MidairActivationController::deactivate()103
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>)109
mrs_uav_controllers::midair_activation_controller::MidairActivationController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)487
mrs_uav_controllers::midair_activation_controller::MidairActivationController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)431
mrs_uav_controllers::midair_activation_controller::MidairActivationController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)151338
mrs_uav_controllers::midair_activation_controller::MidairActivationController::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)15
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&)163
mrs_uav_controllers::midair_activation_controller::MidairActivationController::getStatus()145
+
+
+ + + +
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..7e012446b3 --- /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-12-17 22:35:29Functions: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         109 : 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         109 :   nh_ = nh;
+      90             : 
+      91         109 :   common_handlers_  = common_handlers;
+      92         109 :   private_handlers_ = private_handlers;
+      93             : 
+      94         109 :   _uav_mass_ = common_handlers->getMass();
+      95             : 
+      96         109 :   ros::Time::waitForValid();
+      97             : 
+      98             :   // | ---------- loading params using the parent's nh ---------- |
+      99             : 
+     100         218 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     101             : 
+     102         109 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     103             : 
+     104         109 :   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         109 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/midair_activation_controller.yaml");
+     112         109 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/midair_activation_controller.yaml");
+     113             : 
+     114         109 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     115           0 :     ROS_ERROR("[MidairActivationController]: Could not load all parameters!");
+     116           0 :     return false;
+     117             :   }
+     118             : 
+     119         109 :   uav_mass_difference_ = 0;
+     120             : 
+     121             :   // | ------------------------ profiler ------------------------ |
+     122             : 
+     123         109 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "MidairActivationController", _profiler_enabled_);
+     124             : 
+     125             :   // | ----------------------- finish init ---------------------- |
+     126             : 
+     127         109 :   ROS_INFO("[MidairActivationController]: initialized");
+     128             : 
+     129         109 :   is_initialized_ = true;
+     130             : 
+     131         109 :   return true;
+     132             : }
+     133             : 
+     134             : //}
+     135             : 
+     136             : /* activate() //{ */
+     137             : 
+     138         163 : bool MidairActivationController::activate([[maybe_unused]] const ControlOutput &last_control_output) {
+     139             : 
+     140         163 :   ROS_INFO("[MidairActivationController]: activating");
+     141             : 
+     142         163 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     143             : 
+     144         163 :   hover_throttle_ = mrs_lib::quadratic_throttle_model::forceToThrottle(common_handlers_->throttle_model, _uav_mass_ * common_handlers_->g);
+     145             : 
+     146         163 :   is_active_ = true;
+     147             : 
+     148         163 :   ROS_INFO("[MidairActivationController]: activated, hover throttle %.2f", hover_throttle_);
+     149             : 
+     150         326 :   return true;
+     151             : }
+     152             : 
+     153             : //}
+     154             : 
+     155             : /* deactivate() //{ */
+     156             : 
+     157         103 : void MidairActivationController::deactivate(void) {
+     158             : 
+     159         103 :   is_active_           = false;
+     160         103 :   uav_mass_difference_ = 0;
+     161             : 
+     162         103 :   ROS_INFO("[MidairActivationController]: deactivated");
+     163         103 : }
+     164             : 
+     165             : //}
+     166             : 
+     167             : /* updateInactive() //{ */
+     168             : 
+     169      151338 : void MidairActivationController::updateInactive(const mrs_msgs::UavState &                                      uav_state,
+     170             :                                                 [[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &tracker_command) {
+     171             : 
+     172      151338 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     173      151338 : }
+     174             : 
+     175             : //}
+     176             : 
+     177             : /* //{ updateWhenAcctive() */
+     178             : 
+     179         487 : MidairActivationController::ControlOutput MidairActivationController::updateActive(const mrs_msgs::UavState &      uav_state,
+     180             :                                                                                    const mrs_msgs::TrackerCommand &tracker_command) {
+     181             : 
+     182        1461 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     183             :   mrs_lib::ScopeTimer timer =
+     184        1461 :       mrs_lib::ScopeTimer("MidairActivationController::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     185             : 
+     186         487 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     187             : 
+     188         487 :   if (!is_active_) {
+     189           0 :     return Controller::ControlOutput();
+     190             :   }
+     191             : 
+     192             :   // | --------------- prepare the control output --------------- |
+     193             : 
+     194         974 :   ControlOutput control_output;
+     195             : 
+     196         487 :   control_output.diagnostics.controller = "MidairActivationController";
+     197             : 
+     198         487 :   auto highest_modality = common::getHighestOuput(common_handlers_->control_output_modalities);
+     199             : 
+     200         487 :   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         487 :   switch (highest_modality.value()) {
+     208             : 
+     209           3 :     case common::POSITION: {
+     210             : 
+     211           6 :       mrs_msgs::HwApiPositionCmd cmd;
+     212             : 
+     213           3 :       cmd.header.stamp    = ros::Time::now();
+     214           3 :       cmd.header.frame_id = uav_state.header.frame_id;
+     215             : 
+     216           3 :       cmd.position.x = uav_state.pose.position.x;
+     217           3 :       cmd.position.y = uav_state.pose.position.y;
+     218           3 :       cmd.position.z = uav_state.pose.position.z;
+     219             : 
+     220           3 :       cmd.heading = getHeadingSafely(uav_state, tracker_command);
+     221             : 
+     222           3 :       control_output.control_output = cmd;
+     223             : 
+     224           3 :       break;
+     225             :     }
+     226             : 
+     227           6 :     case common::VELOCITY_HDG: {
+     228             : 
+     229          12 :       mrs_msgs::HwApiVelocityHdgCmd cmd;
+     230             : 
+     231           6 :       cmd.header.stamp    = ros::Time::now();
+     232           6 :       cmd.header.frame_id = uav_state.header.frame_id;
+     233             : 
+     234           6 :       cmd.velocity.x = uav_state.velocity.linear.x;
+     235           6 :       cmd.velocity.y = uav_state.velocity.linear.y;
+     236           6 :       cmd.velocity.z = uav_state.velocity.linear.z;
+     237             : 
+     238           6 :       cmd.heading = getHeadingSafely(uav_state, tracker_command);
+     239             : 
+     240           6 :       control_output.control_output = cmd;
+     241             : 
+     242           6 :       break;
+     243             :     }
+     244             : 
+     245          15 :     case common::VELOCITY_HDG_RATE: {
+     246             : 
+     247          30 :       mrs_msgs::HwApiVelocityHdgRateCmd cmd;
+     248             : 
+     249          15 :       cmd.header.stamp    = ros::Time::now();
+     250          15 :       cmd.header.frame_id = uav_state.header.frame_id;
+     251             : 
+     252          15 :       cmd.velocity.x = uav_state.velocity.linear.x;
+     253          15 :       cmd.velocity.y = uav_state.velocity.linear.y;
+     254          15 :       cmd.velocity.z = uav_state.velocity.linear.z;
+     255             : 
+     256          15 :       cmd.heading_rate = 0;
+     257             : 
+     258          15 :       control_output.control_output = cmd;
+     259             : 
+     260          15 :       break;
+     261             :     }
+     262             : 
+     263           6 :     case common::ACCELERATION_HDG: {
+     264             : 
+     265          12 :       mrs_msgs::HwApiAccelerationHdgCmd cmd;
+     266             : 
+     267           6 :       cmd.header.stamp    = ros::Time::now();
+     268           6 :       cmd.header.frame_id = uav_state.header.frame_id;
+     269             : 
+     270           6 :       cmd.acceleration.x = uav_state.acceleration.linear.x;
+     271           6 :       cmd.acceleration.y = uav_state.acceleration.linear.y;
+     272           6 :       cmd.acceleration.z = uav_state.acceleration.linear.z;
+     273             : 
+     274           6 :       cmd.heading = getHeadingSafely(uav_state, tracker_command);
+     275             : 
+     276           6 :       control_output.control_output = cmd;
+     277             : 
+     278           6 :       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         370 :     case common::ATTITUDE: {
+     300             : 
+     301         370 :       mrs_msgs::HwApiAttitudeCmd cmd;
+     302             : 
+     303         370 :       cmd.stamp = ros::Time::now();
+     304             : 
+     305         370 :       cmd.orientation = uav_state.pose.orientation;
+     306         370 :       cmd.throttle    = hover_throttle_;
+     307             : 
+     308         370 :       control_output.control_output = cmd;
+     309             : 
+     310         370 :       break;
+     311             :     }
+     312             : 
+     313          35 :     case common::ATTITUDE_RATE: {
+     314             : 
+     315          35 :       mrs_msgs::HwApiAttitudeRateCmd cmd;
+     316             : 
+     317          35 :       cmd.stamp = ros::Time::now();
+     318             : 
+     319          35 :       cmd.body_rate.x = 0;
+     320          35 :       cmd.body_rate.y = 0;
+     321          35 :       cmd.body_rate.z = 0;
+     322             : 
+     323          35 :       cmd.throttle = hover_throttle_;
+     324             : 
+     325          35 :       control_output.control_output = cmd;
+     326             : 
+     327          35 :       break;
+     328             :     }
+     329             : 
+     330          22 :     case common::CONTROL_GROUP: {
+     331             : 
+     332          22 :       mrs_msgs::HwApiControlGroupCmd cmd;
+     333             : 
+     334          22 :       cmd.stamp = ros::Time::now();
+     335             : 
+     336          22 :       cmd.roll     = 0;
+     337          22 :       cmd.pitch    = 0;
+     338          22 :       cmd.yaw      = 0;
+     339          22 :       cmd.throttle = hover_throttle_;
+     340             : 
+     341          22 :       control_output.control_output = cmd;
+     342             : 
+     343          22 :       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         487 :   return control_output;
+     363             : }  // namespace midair_activation_controller
+     364             : 
+     365             : //}
+     366             : 
+     367             : /* getStatus() //{ */
+     368             : 
+     369         145 : const mrs_msgs::ControllerStatus MidairActivationController::getStatus() {
+     370             : 
+     371         145 :   mrs_msgs::ControllerStatus controller_status;
+     372             : 
+     373         145 :   controller_status.active = is_active_;
+     374             : 
+     375         145 :   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         431 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr MidairActivationController::setConstraints([
+     397             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &constraints) {
+     398             : 
+     399         431 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+     400             : }
+     401             : 
+     402             : //}
+     403             : 
+     404             : /* getHeadingSafely() //{ */
+     405             : 
+     406          15 : double MidairActivationController::getHeadingSafely(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command) {
+     407             : 
+     408             :   try {
+     409          15 :     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         109 : 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-12-17 22:35:29Functions: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()109
mrs_uav_controllers::mpc_controller::MpcController::deactivate()174
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>)218
mrs_uav_controllers::mpc_controller::MpcController::callbackDrs(mrs_uav_controllers::mpc_controllerConfig&, unsigned int)218
mrs_uav_controllers::mpc_controller::MpcController::activate(mrs_uav_managers::Controller::ControlOutput const&)231
mrs_uav_controllers::mpc_controller::MpcController::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)280
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&)783
mrs_uav_controllers::mpc_controller::MpcController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)862
mrs_uav_controllers::mpc_controller::MpcController::getStatus()15538
mrs_uav_controllers::mpc_controller::MpcController::timerGains(ros::TimerEvent const&)16792
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&)71623
mrs_uav_controllers::mpc_controller::MpcController::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)72406
mrs_uav_controllers::mpc_controller::MpcController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)72686
mrs_uav_controllers::mpc_controller::MpcController::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)132792
mrs_uav_controllers::mpc_controller::MpcController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)230964
+
+
+ + + +
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..38d58de6cb --- /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-12-17 22:35:29Functions:161888.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()109
mrs_uav_controllers::mpc_controller::MpcController::deactivate()174
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>)218
mrs_uav_controllers::mpc_controller::MpcController::timerGains(ros::TimerEvent const&)16792
mrs_uav_controllers::mpc_controller::MpcController::callbackDrs(mrs_uav_controllers::mpc_controllerConfig&, unsigned int)218
mrs_uav_controllers::mpc_controller::MpcController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)72686
mrs_uav_controllers::mpc_controller::MpcController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)862
mrs_uav_controllers::mpc_controller::MpcController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)230964
mrs_uav_controllers::mpc_controller::MpcController::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)72406
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&)783
mrs_uav_controllers::mpc_controller::MpcController::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)132792
mrs_uav_controllers::mpc_controller::MpcController::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)280
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&)71623
mrs_uav_controllers::mpc_controller::MpcController::activate(mrs_uav_managers::Controller::ControlOutput const&)231
mrs_uav_controllers::mpc_controller::MpcController::getStatus()15538
+
+
+ + + +
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..9fa9134ed8 --- /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-12-17 22:35:29Functions: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         218 : 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         218 :   nh_ = nh;
+     260             : 
+     261         218 :   common_handlers_  = common_handlers;
+     262         218 :   private_handlers_ = private_handlers;
+     263             : 
+     264         218 :   _uav_mass_ = common_handlers_->getMass();
+     265         218 :   name_      = private_handlers_->runtime_name;
+     266             : 
+     267         218 :   ros::Time::waitForValid();
+     268             : 
+     269             :   // | ---------- loading params using the parent's nh ---------- |
+     270             : 
+     271         436 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     272             : 
+     273         218 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     274             : 
+     275         218 :   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         218 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/mpc_controller.yaml");
+     283         218 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/mpc_controller.yaml");
+     284         218 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/" + private_handlers->name_space + ".yaml");
+     285         218 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/" + private_handlers->name_space + ".yaml");
+     286             : 
+     287         436 :   const std::string yaml_namespace = "mrs_uav_controllers/" + private_handlers_->name_space + "/";
+     288             : 
+     289             :   // load the dynamicall model parameters
+     290         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_model/number_of_states", _n_states_);
+     291         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_model/dt1", _dt1_);
+     292         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_model/dt2", _dt2_);
+     293             : 
+     294         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizon_length", _horizon_length_);
+     295             : 
+     296         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/max_speed", _max_speed_horizontal_);
+     297         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/max_acceleration", _max_acceleration_horizontal_);
+     298         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/max_jerk", _max_jerk_);
+     299             : 
+     300         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/Q", _mat_Q_);
+     301         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/S", _mat_S_);
+     302             : 
+     303         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/max_speed", _max_speed_vertical_);
+     304         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/max_acceleration", _max_acceleration_vertical_);
+     305         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/max_u", _max_u_vertical_);
+     306             : 
+     307         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/Q", _mat_Q_z_);
+     308         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/S", _mat_S_z_);
+     309             : 
+     310         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/solver/verbose", _mpc_solver_verbose_);
+     311         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/solver/max_iterations", _mpc_solver_max_iterations_);
+     312             : 
+     313             :   // | ------------------------- rampup ------------------------- |
+     314             : 
+     315         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/rampup/enabled", _rampup_enabled_);
+     316         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/rampup/speed", _rampup_speed_);
+     317             : 
+     318             :   // | --------------------- integral gains --------------------- |
+     319             : 
+     320         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/integral_gains/kiw", gains_.kiwxy);
+     321         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/integral_gains/kib", gains_.kibxy);
+     322             : 
+     323             :   // integrator limits
+     324         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/integral_gains/kiw_lim", gains_.kiwxy_lim);
+     325         218 :   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         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/attitude/roll_pitch", gains_.kq_roll_pitch);
+     331         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/attitude/yaw", gains_.kq_yaw);
+     332             : 
+     333             :   // attitude rate gains
+     334         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/attitude_rate/roll_pitch", gains_.kw_rp);
+     335         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/attitude_rate/yaw", gains_.kw_y);
+     336             : 
+     337             :   // mass estimator
+     338         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/mass_estimator/km", gains_.km);
+     339         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/mass_estimator/km_lim", gains_.km_lim);
+     340             : 
+     341             :   // constraints
+     342         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/constraints/tilt_angle_failsafe/enabled", _tilt_angle_failsafe_enabled_);
+     343         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/constraints/tilt_angle_failsafe/limit", _tilt_angle_failsafe_);
+     344             : 
+     345         218 :   _tilt_angle_failsafe_ = M_PI * (_tilt_angle_failsafe_ / 180.0);
+     346             : 
+     347         218 :   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         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/constraints/throttle_saturation", _throttle_saturation_);
+     353             : 
+     354             :   // gain filtering
+     355         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gain_filtering/perc_change_rate", _gains_filter_change_rate_);
+     356         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gain_filtering/min_change_rate", _gains_filter_min_change_rate_);
+     357         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gain_filtering/rate", _gain_filtering_rate_);
+     358         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gain_filtering/gain_mute_coefficient", _gain_mute_coefficient_);
+     359             : 
+     360             :   // angular rate feed forward
+     361         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/angular_rate_feedforward/jerk", drs_params_.jerk_feedforward);
+     362             : 
+     363             :   // output mode
+     364         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/preferred_output", drs_params_.preferred_output_mode);
+     365             : 
+     366             :   // | ------------------- position pid params ------------------ |
+     367             : 
+     368         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/p", _pos_pid_p_);
+     369         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/i", _pos_pid_i_);
+     370         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/d", _pos_pid_d_);
+     371             : 
+     372         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/p", _hdg_pid_p_);
+     373         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/i", _hdg_pid_i_);
+     374         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/d", _hdg_pid_d_);
+     375             : 
+     376         218 :   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         218 :   if (!(drs_params_.preferred_output_mode == OUTPUT_ACTUATORS || drs_params_.preferred_output_mode == OUTPUT_CONTROL_GROUP ||
+     384         218 :         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         218 :   uav_mass_difference_ = 0;
+     390         218 :   Iw_w_                = Eigen::Vector2d::Zero(2);
+     391         218 :   Ib_b_                = Eigen::Vector2d::Zero(2);
+     392             : 
+     393             :   // | ----------------- prepare the MPC solver ----------------- |
+     394             : 
+     395         218 :   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         218 :                                                                             _dt2_, 0, 1.0);
+     397         218 :   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         218 :                                                                             _dt2_, 0, 1.0);
+     399         218 :   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         218 :                                                                             _dt1_, _dt2_, 0.5, 0.5);
+     401             : 
+     402             :   // | --------------- dynamic reconfigure server --------------- |
+     403             : 
+     404         218 :   drs_params_.kiwxy         = gains_.kiwxy;
+     405         218 :   drs_params_.kibxy         = gains_.kibxy;
+     406         218 :   drs_params_.kq_roll_pitch = gains_.kq_roll_pitch;
+     407         218 :   drs_params_.kq_yaw        = gains_.kq_yaw;
+     408         218 :   drs_params_.km            = gains_.km;
+     409         218 :   drs_params_.km_lim        = gains_.km_lim;
+     410         218 :   drs_params_.kiwxy_lim     = gains_.kiwxy_lim;
+     411         218 :   drs_params_.kibxy_lim     = gains_.kibxy_lim;
+     412             : 
+     413         218 :   drs_.reset(new Drs_t(mutex_drs_, nh_));
+     414         218 :   drs_->updateConfig(drs_params_);
+     415         218 :   Drs_t::CallbackType f = boost::bind(&MpcController::callbackDrs, this, _1, _2);
+     416         218 :   drs_->setCallback(f);
+     417             : 
+     418             :   // | --------------------- service servers -------------------- |
+     419             : 
+     420         218 :   service_set_integral_terms_ = nh_.advertiseService("set_integral_terms_in", &MpcController::callbackSetIntegralTerms, this);
+     421             : 
+     422             :   // | ------------------------- timers ------------------------- |
+     423             : 
+     424         218 :   timer_gains_ = nh_.createTimer(ros::Rate(_gain_filtering_rate_), &MpcController::timerGains, this, false, false);
+     425             : 
+     426             :   // | ---------------------- position pid ---------------------- |
+     427             : 
+     428         218 :   position_pid_x_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     429         218 :   position_pid_y_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     430         218 :   position_pid_z_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     431         218 :   position_pid_heading_.setParams(_hdg_pid_p_, _hdg_pid_d_, _hdg_pid_i_, -1, 0.1);
+     432             : 
+     433             :   // | ------------------------ profiler ------------------------ |
+     434             : 
+     435         218 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "MpcController", _profiler_enabled_);
+     436             : 
+     437             :   // | ----------------------- finish init ---------------------- |
+     438             : 
+     439         218 :   ROS_INFO("[%s]: initialized", this->name_.c_str());
+     440             : 
+     441         218 :   is_initialized_ = true;
+     442             : 
+     443         218 :   return true;
+     444             : }
+     445             : 
+     446             : //}
+     447             : 
+     448             : /* //{ activate() */
+     449             : 
+     450         231 : bool MpcController::activate(const ControlOutput &last_control_output) {
+     451             : 
+     452         231 :   activation_control_output_ = last_control_output;
+     453             : 
+     454         231 :   double activation_mass = _uav_mass_;
+     455             : 
+     456         231 :   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         231 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+     463             : 
+     464         231 :   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         231 :   auto throttle_last_controller = common::extractThrottle(activation_control_output_);
+     479             : 
+     480             :   // rampup check
+     481         231 :   if (_rampup_enabled_ && throttle_last_controller) {
+     482             : 
+     483          85 :     double hover_throttle = mrs_lib::quadratic_throttle_model::forceToThrottle(common_handlers_->throttle_model, activation_mass * common_handlers_->g);
+     484             : 
+     485          85 :     double throttle_difference = hover_throttle - throttle_last_controller.value();
+     486             : 
+     487          85 :     if (throttle_difference > 0) {
+     488          36 :       rampup_direction_ = 1;
+     489          49 :     } else if (throttle_difference < 0) {
+     490           3 :       rampup_direction_ = -1;
+     491             :     } else {
+     492          46 :       rampup_direction_ = 0;
+     493             :     }
+     494             : 
+     495          85 :     ROS_INFO("[%s]: activating rampup with initial throttle: %.4f, target: %.4f", name_.c_str(), throttle_last_controller.value(), hover_throttle);
+     496             : 
+     497          85 :     rampup_active_     = true;
+     498          85 :     rampup_start_time_ = ros::Time::now();
+     499          85 :     rampup_last_time_  = ros::Time::now();
+     500          85 :     rampup_throttle_   = throttle_last_controller.value();
+     501             : 
+     502          85 :     rampup_duration_ = std::abs(throttle_difference) / _rampup_speed_;
+     503             :   }
+     504             : 
+     505         231 :   first_iteration_ = true;
+     506         231 :   mute_gains_      = true;
+     507             : 
+     508         231 :   timer_gains_.start();
+     509             : 
+     510         231 :   ROS_INFO("[%s]: activated", this->name_.c_str());
+     511             : 
+     512         231 :   is_active_ = true;
+     513             : 
+     514         231 :   return true;
+     515             : }
+     516             : 
+     517             : //}
+     518             : 
+     519             : /* //{ deactivate() */
+     520             : 
+     521         174 : void MpcController::deactivate(void) {
+     522             : 
+     523         174 :   is_active_           = false;
+     524         174 :   first_iteration_     = false;
+     525         174 :   uav_mass_difference_ = 0;
+     526             : 
+     527         174 :   timer_gains_.stop();
+     528             : 
+     529         174 :   ROS_INFO("[%s]: deactivated", this->name_.c_str());
+     530         174 : }
+     531             : 
+     532             : //}
+     533             : 
+     534             : /* updateInactive() //{ */
+     535             : 
+     536      230964 : void MpcController::updateInactive(const mrs_msgs::UavState &uav_state, [[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &tracker_command) {
+     537             : 
+     538      230964 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     539             : 
+     540      230964 :   last_update_time_ = uav_state.header.stamp;
+     541             : 
+     542      230964 :   first_iteration_ = false;
+     543      230964 : }
+     544             : 
+     545             : //}
+     546             : 
+     547             : /* //{ updateWhenAcctive() */
+     548             : 
+     549       72686 : MpcController::ControlOutput MpcController::updateActive(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command) {
+     550             : 
+     551      218058 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("updateActive");
+     552      218058 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcController::updateActive", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     553             : 
+     554      145372 :   auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+     555             : 
+     556       72686 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     557             : 
+     558       72686 :   last_control_output_.desired_heading_rate          = {};
+     559       72686 :   last_control_output_.desired_orientation           = {};
+     560       72686 :   last_control_output_.desired_unbiased_acceleration = {};
+     561       72686 :   last_control_output_.control_output                = {};
+     562             : 
+     563       72686 :   if (!is_active_) {
+     564           0 :     return last_control_output_;
+     565             :   }
+     566             : 
+     567             :   // | -------------------- calculate the dt -------------------- |
+     568             : 
+     569             :   double dt;
+     570             : 
+     571       72686 :   if (first_iteration_) {
+     572          84 :     dt               = 0.01;
+     573          84 :     first_iteration_ = false;
+     574             :   } else {
+     575       72602 :     dt = (uav_state.header.stamp - last_update_time_).toSec();
+     576             :   }
+     577             : 
+     578       72686 :   last_update_time_ = uav_state.header.stamp;
+     579             : 
+     580       72686 :   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       72686 :   auto lowest_modality = common::getLowestOuput(common_handlers_->control_output_modalities);
+     590             : 
+     591       72686 :   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       72686 :   if (drs_params.preferred_output_mode == OUTPUT_ATTITUDE_RATE && common_handlers_->control_output_modalities.attitude_rate) {
+     601       66857 :     ROS_DEBUG_THROTTLE(1.0, "[%s]: prioritizing attitude rate output", name_.c_str());
+     602       66857 :     lowest_modality = common::ATTITUDE_RATE;
+     603        5829 :   } 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        5829 :   } 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        5829 :   } 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       72686 :   switch (lowest_modality.value()) {
+     615             : 
+     616         280 :     case common::POSITION: {
+     617         280 :       positionPassthrough(uav_state, tracker_command);
+     618         280 :       break;
+     619             :     }
+     620             : 
+     621         265 :     case common::VELOCITY_HDG: {
+     622         265 :       PIDVelocityOutput(uav_state, tracker_command, common::VELOCITY_HDG, dt);
+     623         265 :       break;
+     624             :     }
+     625             : 
+     626         518 :     case common::VELOCITY_HDG_RATE: {
+     627         518 :       PIDVelocityOutput(uav_state, tracker_command, common::VELOCITY_HDG_RATE, dt);
+     628         518 :       break;
+     629             :     }
+     630             : 
+     631         507 :     case common::ACCELERATION_HDG: {
+     632         507 :       MPC(uav_state, tracker_command, dt, common::ACCELERATION_HDG);
+     633         507 :       break;
+     634             :     }
+     635             : 
+     636         560 :     case common::ACCELERATION_HDG_RATE: {
+     637         560 :       MPC(uav_state, tracker_command, dt, common::ACCELERATION_HDG_RATE);
+     638         560 :       break;
+     639             :     }
+     640             : 
+     641        1143 :     case common::ATTITUDE: {
+     642        1143 :       MPC(uav_state, tracker_command, dt, common::ATTITUDE);
+     643        1143 :       break;
+     644             :     }
+     645             : 
+     646       66857 :     case common::ATTITUDE_RATE: {
+     647       66857 :       MPC(uav_state, tracker_command, dt, common::ATTITUDE_RATE);
+     648       66857 :       break;
+     649             :     }
+     650             : 
+     651        1254 :     case common::CONTROL_GROUP: {
+     652        1254 :       MPC(uav_state, tracker_command, dt, common::CONTROL_GROUP);
+     653        1254 :       break;
+     654             :     }
+     655             : 
+     656        1302 :     case common::ACTUATORS_CMD: {
+     657        1302 :       MPC(uav_state, tracker_command, dt, common::ACTUATORS_CMD);
+     658        1302 :       break;
+     659             :     }
+     660             : 
+     661           0 :     default: {
+     662           0 :       break;
+     663             :     }
+     664             :   }
+     665             : 
+     666       72686 :   return last_control_output_;
+     667             : }
+     668             : 
+     669             : //}
+     670             : 
+     671             : /* //{ getStatus() */
+     672             : 
+     673       15538 : const mrs_msgs::ControllerStatus MpcController::getStatus() {
+     674             : 
+     675       15538 :   mrs_msgs::ControllerStatus controller_status;
+     676             : 
+     677       15538 :   controller_status.active = is_active_;
+     678             : 
+     679       15538 :   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         862 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr MpcController::setConstraints([
+     740             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &constraints) {
+     741             : 
+     742         862 :   if (!is_initialized_) {
+     743           0 :     return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+     744             :   }
+     745             : 
+     746         862 :   mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_);
+     747             : 
+     748         862 :   ROS_INFO("[%s]: updating constraints", this->name_.c_str());
+     749             : 
+     750        1724 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     751         862 :   res.success = true;
+     752         862 :   res.message = "constraints updated";
+     753             : 
+     754         862 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     755             : }
+     756             : 
+     757             : //}
+     758             : 
+     759             : // --------------------------------------------------------------
+     760             : // |                         controllers                        |
+     761             : // --------------------------------------------------------------
+     762             : 
+     763             : /* Mpc() //{ */
+     764             : 
+     765       71623 : 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      143246 :   auto drs_params  = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+     769       71623 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     770       71623 :   auto gains       = mrs_lib::get_mutexed(mutex_gains_, gains_);
+     771             : 
+     772             :   // | ----------------- get the current heading ---------------- |
+     773             : 
+     774       71623 :   double uav_heading = getHeadingSafely(uav_state, tracker_command);
+     775             : 
+     776             :   // | ------------------- prepare constraints ------------------ |
+     777             : 
+     778       71623 :   double max_speed_horizontal        = _max_speed_horizontal_;
+     779       71623 :   double max_speed_vertical          = _max_speed_vertical_;
+     780       71623 :   double max_acceleration_horizontal = _max_acceleration_horizontal_;
+     781       71623 :   double max_acceleration_vertical   = _max_acceleration_vertical_;
+     782       71623 :   double max_jerk                    = _max_jerk_;
+     783       71623 :   double max_u_vertical              = _max_u_vertical_;
+     784             : 
+     785       71623 :   if (tracker_command.use_full_state_prediction) {
+     786             : 
+     787       51957 :     max_speed_horizontal = 1.5 * (constraints.horizontal_speed);
+     788       51957 :     max_speed_vertical   = 1.5 * (constraints.vertical_ascending_speed < constraints.vertical_descending_speed ? constraints.vertical_ascending_speed
+     789             :                                                                                                              : constraints.vertical_descending_speed);
+     790             : 
+     791       51957 :     max_acceleration_horizontal = 1.5 * (constraints.horizontal_acceleration);
+     792       51957 :     max_acceleration_vertical =
+     793       51957 :         1.5 * (constraints.vertical_ascending_acceleration < constraints.vertical_descending_acceleration ? constraints.vertical_ascending_acceleration
+     794             :                                                                                                           : constraints.vertical_descending_acceleration);
+     795             : 
+     796       51957 :     max_jerk       = 1.5 * constraints.horizontal_jerk;
+     797       51957 :     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       71623 :   Eigen::Vector3d Rp = Eigen::Vector3d::Zero(3);
+     810       71623 :   Eigen::Vector3d Rv = Eigen::Vector3d::Zero(3);
+     811       71623 :   Eigen::Vector3d Ra = Eigen::Vector3d::Zero(3);
+     812             : 
+     813       71623 :   Rp << tracker_command.position.x, tracker_command.position.y, tracker_command.position.z;  // fill the desired position
+     814       71623 :   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       71623 :   Eigen::Vector3d Op(uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z);
+     821       71623 :   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       71623 :   Eigen::Matrix3d R = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+     825             : 
+     826             :   // Ow - UAV angular rate
+     827       71623 :   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       71623 :   Eigen::Vector3d Ep = Rp - Op;
+     836       71623 :   Eigen::Vector3d Ev = Rv - Ov;
+     837             : 
+     838             :   // | ------------------- initial conditions ------------------- |
+     839             : 
+     840      143246 :   Eigen::MatrixXd initial_x = Eigen::MatrixXd::Zero(3, 1);
+     841      143246 :   Eigen::MatrixXd initial_y = Eigen::MatrixXd::Zero(3, 1);
+     842      143246 :   Eigen::MatrixXd initial_z = Eigen::MatrixXd::Zero(3, 1);
+     843             : 
+     844             :   /* initial x //{ */
+     845             : 
+     846             :   {
+     847             :     double acceleration;
+     848             :     double velocity;
+     849       71623 :     double coef = 1.5;
+     850             : 
+     851       71623 :     if (std::abs(uav_state.acceleration.linear.x) < coef * max_acceleration_horizontal) {
+     852       71623 :       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       71623 :     if (std::abs(uav_state.velocity.linear.x) < coef * max_speed_horizontal) {
+     861       71623 :       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       71623 :     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       71623 :     double coef = 1.5;
+     880             : 
+     881       71623 :     if (std::abs(uav_state.acceleration.linear.y) < coef * max_acceleration_horizontal) {
+     882       71623 :       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       71623 :     if (std::abs(uav_state.velocity.linear.y) < coef * max_speed_horizontal) {
+     891       71623 :       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       71623 :     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       71623 :     double coef = 1.5;
+     910             : 
+     911       71623 :     if (std::abs(uav_state.acceleration.linear.z) < coef * max_acceleration_horizontal) {
+     912       71623 :       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       71623 :     if (std::abs(uav_state.velocity.linear.z) < coef * max_speed_vertical) {
+     921       71623 :       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       71623 :     initial_z << uav_state.pose.position.z, velocity, acceleration;
+     930             :   }
+     931             : 
+     932             :   //}
+     933             : 
+     934             :   // | ---------------------- set reference --------------------- |
+     935             : 
+     936      143246 :   Eigen::MatrixXd mpc_reference_x = Eigen::MatrixXd::Zero(_horizon_length_ * _n_states_, 1);
+     937      143246 :   Eigen::MatrixXd mpc_reference_y = Eigen::MatrixXd::Zero(_horizon_length_ * _n_states_, 1);
+     938      143246 :   Eigen::MatrixXd mpc_reference_z = Eigen::MatrixXd::Zero(_horizon_length_ * _n_states_, 1);
+     939             : 
+     940             :   // prepare the full reference vector
+     941       71623 :   if (tracker_command.use_full_state_prediction) {
+     942             : 
+     943       51957 :     mpc_reference_x(0, 0) = tracker_command.position.x;
+     944       51957 :     mpc_reference_y(0, 0) = tracker_command.position.y;
+     945       51957 :     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     1350882 :     for (int i = 1; i < _horizon_length_; i++) {
+     951     1298925 :       mpc_reference_x((i * _n_states_) + 0, 0) = tracker_command.full_state_prediction.position.at(i).x;
+     952     1298925 :       mpc_reference_y((i * _n_states_) + 0, 0) = tracker_command.full_state_prediction.position.at(i).y;
+     953     1298925 :       mpc_reference_z((i * _n_states_) + 0, 0) = tracker_command.full_state_prediction.position.at(i).z;
+     954             :     }
+     955             : 
+     956     1350882 :     for (int i = 1; i < _horizon_length_; i++) {
+     957     1298925 :       mpc_reference_x((i * _n_states_) + 1, 0) = tracker_command.full_state_prediction.velocity.at(i).x;
+     958     1298925 :       mpc_reference_y((i * _n_states_) + 1, 0) = tracker_command.full_state_prediction.velocity.at(i).y;
+     959     1298925 :       mpc_reference_z((i * _n_states_) + 1, 0) = tracker_command.full_state_prediction.velocity.at(i).z;
+     960             :     }
+     961             : 
+     962     1350882 :     for (int i = 1; i < _horizon_length_; i++) {
+     963     1298925 :       mpc_reference_x((i * _n_states_) + 2, 0) = tracker_command.full_state_prediction.acceleration.at(i).x;
+     964     1298925 :       mpc_reference_y((i * _n_states_) + 2, 0) = tracker_command.full_state_prediction.acceleration.at(i).y;
+     965     1298925 :       mpc_reference_z((i * _n_states_) + 2, 0) = tracker_command.full_state_prediction.acceleration.at(i).z;
+     966             :     }
+     967             : 
+     968             :   } else {
+     969             : 
+     970      530982 :     for (int i = 0; i < _horizon_length_; i++) {
+     971      511316 :       mpc_reference_x((i * _n_states_) + 0, 0) = tracker_command.position.x;
+     972      511316 :       mpc_reference_y((i * _n_states_) + 0, 0) = tracker_command.position.y;
+     973      511316 :       mpc_reference_z((i * _n_states_) + 0, 0) = tracker_command.position.z;
+     974             :     }
+     975             :   }
+     976             : 
+     977             :   // | ------------------ set the penalizations ----------------- |
+     978             : 
+     979      143246 :   std::vector<double> temp_Q_horizontal = _mat_Q_;
+     980      143246 :   std::vector<double> temp_Q_vertical   = _mat_Q_z_;
+     981             : 
+     982      143246 :   std::vector<double> temp_S_horizontal = _mat_S_;
+     983      143246 :   std::vector<double> temp_S_vertical   = _mat_S_z_;
+     984             : 
+     985       71623 :   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       71623 :   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       71623 :   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       71623 :   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       71623 :   mpc_solver_x_->setQ(temp_Q_horizontal);
+    1008       71623 :   mpc_solver_x_->setS(temp_S_horizontal);
+    1009       71623 :   mpc_solver_x_->setParams();
+    1010       71623 :   mpc_solver_x_->setLastInput(mpc_solver_x_u_);
+    1011       71623 :   mpc_solver_x_->loadReference(mpc_reference_x);
+    1012       71623 :   mpc_solver_x_->setLimits(max_speed_horizontal, 999, max_acceleration_horizontal, max_jerk, _dt1_, _dt2_);
+    1013       71623 :   mpc_solver_x_->setInitialState(initial_x);
+    1014       71623 :   [[maybe_unused]] int iters_x = mpc_solver_x_->solveMPC();
+    1015       71623 :   mpc_solver_x_u_              = mpc_solver_x_->getFirstControlInput();
+    1016             : 
+    1017       71623 :   mpc_solver_y_->setQ(temp_Q_horizontal);
+    1018       71623 :   mpc_solver_y_->setS(temp_S_horizontal);
+    1019       71623 :   mpc_solver_y_->setParams();
+    1020       71623 :   mpc_solver_y_->setLastInput(mpc_solver_y_u_);
+    1021       71623 :   mpc_solver_y_->loadReference(mpc_reference_y);
+    1022       71623 :   mpc_solver_y_->setLimits(max_speed_horizontal, 999, max_acceleration_horizontal, max_jerk, _dt1_, _dt2_);
+    1023       71623 :   mpc_solver_y_->setInitialState(initial_y);
+    1024       71623 :   [[maybe_unused]] int iters_y = mpc_solver_y_->solveMPC();
+    1025       71623 :   mpc_solver_y_u_              = mpc_solver_y_->getFirstControlInput();
+    1026             : 
+    1027       71623 :   mpc_solver_z_->setQ(temp_Q_vertical);
+    1028       71623 :   mpc_solver_z_->setS(temp_S_vertical);
+    1029       71623 :   mpc_solver_z_->setParams();
+    1030       71623 :   mpc_solver_z_->setLastInput(mpc_solver_z_u_);
+    1031       71623 :   mpc_solver_z_->loadReference(mpc_reference_z);
+    1032       71623 :   mpc_solver_z_->setLimits(max_speed_vertical, max_acceleration_vertical, max_u_vertical, 999.0, _dt1_, _dt2_);
+    1033       71623 :   mpc_solver_z_->setInitialState(initial_z);
+    1034       71623 :   [[maybe_unused]] int iters_z = mpc_solver_z_->solveMPC();
+    1035       71623 :   mpc_solver_z_u_              = mpc_solver_z_->getFirstControlInput();
+    1036             : 
+    1037             :   // | ----------- disable lateral feedback if needed ----------- |
+    1038             : 
+    1039       71623 :   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       71623 :   mute_gains_by_tracker_ = tracker_command.disable_position_gains;
+    1047             : 
+    1048       71623 :   Eigen::Array3d Kw(0, 0, 0);
+    1049       71623 :   Eigen::Array3d Kq;
+    1050             : 
+    1051             :   {
+    1052       71623 :     std::scoped_lock lock(mutex_gains_);
+    1053             : 
+    1054       71623 :     Kq << gains.kq_roll_pitch, gains.kq_roll_pitch, gains.kq_yaw;
+    1055             : 
+    1056       71623 :     Kw(0) = gains.kw_rp;
+    1057       71623 :     Kw(1) = gains.kw_rp;
+    1058       71623 :     Kw(2) = gains.kw_y;
+    1059             :   }
+    1060             : 
+    1061             :   // | ---------- desired orientation matrix and force ---------- |
+    1062             : 
+    1063             :   // get body integral in the world frame
+    1064             : 
+    1065       71623 :   Eigen::Vector2d Ib_w = Eigen::Vector2d(0, 0);
+    1066             : 
+    1067             :   {
+    1068             : 
+    1069      143246 :     geometry_msgs::Vector3Stamped Ib_b_stamped;
+    1070             : 
+    1071       71623 :     Ib_b_stamped.header.stamp    = ros::Time::now();
+    1072       71623 :     Ib_b_stamped.header.frame_id = "fcu_untilted";
+    1073       71623 :     Ib_b_stamped.vector.x        = Ib_b_(0);
+    1074       71623 :     Ib_b_stamped.vector.y        = Ib_b_(1);
+    1075       71623 :     Ib_b_stamped.vector.z        = 0;
+    1076             : 
+    1077      143246 :     auto res = common_handlers_->transformer->transformSingle(Ib_b_stamped, uav_state_.header.frame_id);
+    1078             : 
+    1079       71623 :     if (res) {
+    1080       71623 :       Ib_w(0) = res.value().vector.x;
+    1081       71623 :       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       71623 :   if (tracker_command.use_acceleration && !tracker_command.use_full_state_prediction) {
+    1090        1710 :     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       69913 :     Ra << mpc_solver_x_u_, mpc_solver_y_u_, mpc_solver_z_u_;
+    1093             :   }
+    1094             : 
+    1095       71623 :   double total_mass = _uav_mass_ + uav_mass_difference_;
+    1096             : 
+    1097       71623 :   Eigen::Vector3d feed_forward = total_mass * (Eigen::Vector3d(0, 0, common_handlers_->g) + Ra);
+    1098             : 
+    1099       71623 :   Eigen::Vector3d integral_feedback;
+    1100             :   {
+    1101       71623 :     std::scoped_lock lock(mutex_integrals_);
+    1102             : 
+    1103       71623 :     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      143246 :     std::scoped_lock lock(mutex_gains_, mutex_integrals_);
+    1118             : 
+    1119       71623 :     Eigen::Vector3d integration_switch(1, 1, 0);
+    1120             : 
+    1121             :     // integrate the world error
+    1122             : 
+    1123             :     // antiwindup
+    1124       71623 :     double temp_gain = gains.kiwxy;
+    1125       71623 :     if (!tracker_command.disable_antiwindups) {
+    1126       62252 :       if (rampup_active_ || sqrt(pow(uav_state.velocity.linear.x, 2) + pow(uav_state.velocity.linear.y, 2)) > 0.3) {
+    1127       25963 :         temp_gain = 0;
+    1128       25963 :         ROS_DEBUG_THROTTLE(1.0, "[%s]: anti-windup for world integral kicks in", this->name_.c_str());
+    1129             :       }
+    1130             :     }
+    1131             : 
+    1132       71623 :     if (integral_terms_enabled_) {
+    1133       71623 :       if (tracker_command.use_position_horizontal) {
+    1134       71623 :         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       71623 :     bool world_integral_saturated = false;
+    1142       71623 :     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       71623 :     } else if (Iw_w_(0) > gains.kiwxy_lim) {
+    1146           0 :       Iw_w_(0)                 = gains.kiwxy_lim;
+    1147           0 :       world_integral_saturated = true;
+    1148       71623 :     } 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       71623 :     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       71623 :     world_integral_saturated = false;
+    1159       71623 :     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       71623 :     } else if (Iw_w_(1) > gains.kiwxy_lim) {
+    1163           0 :       Iw_w_(1)                 = gains.kiwxy_lim;
+    1164           0 :       world_integral_saturated = true;
+    1165       71623 :     } 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       71623 :     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      143246 :     std::scoped_lock lock(mutex_gains_);
+    1181             : 
+    1182       71623 :     Eigen::Vector2d Ep_fcu_untilted = Eigen::Vector2d(0, 0);  // position error in the untilted frame of the UAV
+    1183       71623 :     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      143246 :       geometry_msgs::Vector3Stamped Ep_stamped;
+    1189             : 
+    1190       71623 :       Ep_stamped.header.stamp    = ros::Time::now();
+    1191       71623 :       Ep_stamped.header.frame_id = uav_state_.header.frame_id;
+    1192       71623 :       Ep_stamped.vector.x        = Ep(0);
+    1193       71623 :       Ep_stamped.vector.y        = Ep(1);
+    1194       71623 :       Ep_stamped.vector.z        = Ep(2);
+    1195             : 
+    1196      214869 :       auto res = common_handlers_->transformer->transformSingle(Ep_stamped, "fcu_untilted");
+    1197             : 
+    1198       71623 :       if (res) {
+    1199       71623 :         Ep_fcu_untilted(0) = res.value().vector.x;
+    1200       71623 :         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      143246 :       geometry_msgs::Vector3Stamped Ev_stamped;
+    1209             : 
+    1210       71623 :       Ev_stamped.header.stamp    = ros::Time::now();
+    1211       71623 :       Ev_stamped.header.frame_id = uav_state_.header.frame_id;
+    1212       71623 :       Ev_stamped.vector.x        = Ev(0);
+    1213       71623 :       Ev_stamped.vector.y        = Ev(1);
+    1214       71623 :       Ev_stamped.vector.z        = Ev(2);
+    1215             : 
+    1216      214869 :       auto res = common_handlers_->transformer->transformSingle(Ev_stamped, "fcu_untilted");
+    1217             : 
+    1218       71623 :       if (res) {
+    1219       71623 :         Ev_fcu_untilted(0) = res.value().vector.x;
+    1220       71623 :         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       71623 :     double temp_gain = gains.kibxy;
+    1230       71623 :     if (!tracker_command.disable_antiwindups) {
+    1231       62252 :       if (rampup_active_ || sqrt(pow(uav_state.velocity.linear.x, 2) + pow(uav_state.velocity.linear.y, 2)) > 0.3) {
+    1232       25963 :         temp_gain = 0;
+    1233       25963 :         ROS_DEBUG_THROTTLE(1.0, "[%s]: anti-windup for body integral kicks in", this->name_.c_str());
+    1234             :       }
+    1235             :     }
+    1236             : 
+    1237       71623 :     if (integral_terms_enabled_) {
+    1238       71623 :       if (tracker_command.use_position_horizontal) {
+    1239       71623 :         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       71623 :     bool body_integral_saturated = false;
+    1247       71623 :     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       71623 :     } else if (Ib_b_(0) > gains.kibxy_lim) {
+    1251           0 :       Ib_b_(0)                = gains.kibxy_lim;
+    1252           0 :       body_integral_saturated = true;
+    1253       71623 :     } 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       71623 :     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       71623 :     body_integral_saturated = false;
+    1264       71623 :     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       71623 :     } else if (Ib_b_(1) > gains.kibxy_lim) {
+    1268           0 :       Ib_b_(1)                = gains.kibxy_lim;
+    1269           0 :       body_integral_saturated = true;
+    1270       71623 :     } 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       71623 :     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       71623 :   Eigen::Vector3d des_acc = integral_feedback / total_mass + Ra;
+    1283             : 
+    1284       71623 :   if (output_modality == common::ACCELERATION_HDG || output_modality == common::ACCELERATION_HDG_RATE) {
+    1285             : 
+    1286        1067 :     if (output_modality == common::ACCELERATION_HDG) {
+    1287             : 
+    1288        1014 :       mrs_msgs::HwApiAccelerationHdgCmd cmd;
+    1289             : 
+    1290         507 :       cmd.acceleration.x = des_acc(0);
+    1291         507 :       cmd.acceleration.y = des_acc(1);
+    1292         507 :       cmd.acceleration.z = des_acc(2);
+    1293             : 
+    1294         507 :       cmd.heading = tracker_command.heading;
+    1295             : 
+    1296         507 :       last_control_output_.control_output = cmd;
+    1297             : 
+    1298             :     } else {
+    1299             : 
+    1300         560 :       double des_hdg_ff = 0;
+    1301             : 
+    1302         560 :       if (tracker_command.use_heading_rate) {
+    1303         560 :         des_hdg_ff = tracker_command.heading_rate;
+    1304             :       }
+    1305             : 
+    1306        1120 :       mrs_msgs::HwApiAccelerationHdgRateCmd cmd;
+    1307             : 
+    1308         560 :       cmd.acceleration.x = des_acc(0);
+    1309         560 :       cmd.acceleration.y = des_acc(1);
+    1310         560 :       cmd.acceleration.z = des_acc(2);
+    1311             : 
+    1312         560 :       position_pid_heading_.setSaturation(constraints.heading_speed);
+    1313             : 
+    1314         560 :       double hdg_err = mrs_lib::geometry::sradians::diff(tracker_command.heading, uav_heading);
+    1315             : 
+    1316         560 :       double des_hdg_rate = position_pid_heading_.update(hdg_err, dt) + des_hdg_ff;
+    1317             : 
+    1318         560 :       cmd.heading_rate = des_hdg_rate;
+    1319             : 
+    1320         560 :       last_control_output_.desired_heading_rate = des_hdg_rate;
+    1321             : 
+    1322         560 :       last_control_output_.control_output = cmd;
+    1323             :     }
+    1324             : 
+    1325             :     // | -------------- unbiased desired acceleration ------------- |
+    1326             : 
+    1327        1067 :     Eigen::Vector3d unbiased_des_acc(0, 0, 0);
+    1328             : 
+    1329             :     {
+    1330             : 
+    1331        2134 :       geometry_msgs::Vector3Stamped world_accel;
+    1332             : 
+    1333        1067 :       world_accel.header.stamp    = ros::Time::now();
+    1334        1067 :       world_accel.header.frame_id = uav_state.header.frame_id;
+    1335        1067 :       world_accel.vector.x        = Ra(0);
+    1336        1067 :       world_accel.vector.y        = Ra(1);
+    1337        1067 :       world_accel.vector.z        = Ra(2);
+    1338             : 
+    1339        3201 :       auto res = common_handlers_->transformer->transformSingle(world_accel, "fcu");
+    1340             : 
+    1341        1067 :       if (res) {
+    1342        1067 :         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        1067 :     last_control_output_.desired_unbiased_acceleration = unbiased_des_acc;
+    1348             : 
+    1349             :     // | ----------------- fill in the diagnostics ---------------- |
+    1350             : 
+    1351        1067 :     last_control_output_.diagnostics.ramping_up = false;
+    1352             : 
+    1353        1067 :     last_control_output_.diagnostics.mass_estimator  = false;
+    1354        1067 :     last_control_output_.diagnostics.mass_difference = 0;
+    1355        1067 :     last_control_output_.diagnostics.total_mass      = total_mass;
+    1356             : 
+    1357        1067 :     last_control_output_.diagnostics.disturbance_estimator = true;
+    1358             : 
+    1359        1067 :     last_control_output_.diagnostics.disturbance_bx_b = -Ib_b_(0);
+    1360        1067 :     last_control_output_.diagnostics.disturbance_by_b = -Ib_b_(1);
+    1361             : 
+    1362        1067 :     last_control_output_.diagnostics.disturbance_bx_w = -Ib_w(0);
+    1363        1067 :     last_control_output_.diagnostics.disturbance_by_w = -Ib_w(1);
+    1364             : 
+    1365        1067 :     last_control_output_.diagnostics.disturbance_wx_w = -Iw_w_(0);
+    1366        1067 :     last_control_output_.diagnostics.disturbance_wy_w = -Iw_w_(1);
+    1367             : 
+    1368        1067 :     last_control_output_.diagnostics.controller_enforcing_constraints = !tracker_command.use_full_state_prediction;
+    1369             : 
+    1370        1067 :     last_control_output_.diagnostics.horizontal_speed_constraint = 0.5 * max_speed_horizontal;
+    1371        1067 :     last_control_output_.diagnostics.horizontal_acc_constraint   = 0.5 * max_acceleration_horizontal;
+    1372             : 
+    1373        1067 :     last_control_output_.diagnostics.vertical_asc_speed_constraint = 0.5 * max_speed_vertical;
+    1374        1067 :     last_control_output_.diagnostics.vertical_asc_acc_constraint   = 0.5 * max_acceleration_vertical;
+    1375             : 
+    1376        1067 :     last_control_output_.diagnostics.vertical_desc_speed_constraint = 0.5 * max_speed_vertical;
+    1377        1067 :     last_control_output_.diagnostics.vertical_desc_acc_constraint   = 0.5 * max_acceleration_vertical;
+    1378             : 
+    1379        1067 :     last_control_output_.diagnostics.controller = name_;
+    1380             : 
+    1381        1067 :     return;
+    1382             :   }
+    1383             : 
+    1384             :   /* mass estimatior //{ */
+    1385             : 
+    1386             :   // --------------------------------------------------------------
+    1387             :   // |                integrate the mass difference               |
+    1388             :   // --------------------------------------------------------------
+    1389             : 
+    1390             :   {
+    1391      141112 :     std::scoped_lock lock(mutex_gains_);
+    1392             : 
+    1393             :     // antiwindup
+    1394       70556 :     double temp_gain = gains.km;
+    1395      157683 :     if (rampup_active_ ||
+    1396       87127 :         (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       16783 :       temp_gain = 0;
+    1398       16783 :       ROS_DEBUG_THROTTLE(1.0, "[%s]: anti-windup for the mass kicks in", this->name_.c_str());
+    1399             :     }
+    1400             : 
+    1401       70556 :     if (tracker_command.use_position_vertical) {
+    1402       70556 :       uav_mass_difference_ += temp_gain * Ep(2) * dt;
+    1403             :     }
+    1404             : 
+    1405             :     // saturate the mass estimator
+    1406       70556 :     bool uav_mass_saturated = false;
+    1407       70556 :     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       70556 :     } else if (uav_mass_difference_ > gains.km_lim) {
+    1411           0 :       uav_mass_difference_ = gains.km_lim;
+    1412           0 :       uav_mass_saturated   = true;
+    1413       70556 :     } 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       70556 :     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       70556 :   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       70556 :   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       70556 :   double tilt_safety_limit = _tilt_angle_failsafe_enabled_ ? _tilt_angle_failsafe_ : std::numeric_limits<double>::max();
+    1441             : 
+    1442       70556 :   auto f_normed_sanitized = common::sanitizeDesiredForce(f.normalized(), tilt_safety_limit, constraints.tilt, "MpcController");
+    1443             : 
+    1444       70556 :   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       70556 :   Eigen::Vector3d f_normed = f_normed_sanitized.value();
+    1458             : 
+    1459             :   // --------------------------------------------------------------
+    1460             :   // |               desired orientation + throttle               |
+    1461             :   // --------------------------------------------------------------
+    1462             : 
+    1463             :   // | ------------------- desired orientation ------------------ |
+    1464             : 
+    1465       70556 :   Eigen::Matrix3d Rd;
+    1466             : 
+    1467       70556 :   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       70556 :     Eigen::Vector3d bxd;  // desired heading vector
+    1484             : 
+    1485       70556 :     if (tracker_command.use_heading) {
+    1486       70556 :       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       70556 :     Rd = common::so3transform(f_normed, bxd, false);
+    1493             :   }
+    1494             : 
+    1495             :   // | -------------------- desired throttle -------------------- |
+    1496             : 
+    1497       70556 :   double desired_thrust_force = f.dot(R.col(2));
+    1498       70556 :   double throttle             = 0;
+    1499             : 
+    1500       70556 :   if (rampup_active_) {
+    1501             : 
+    1502             :     // deactivate the rampup when the times up
+    1503        1059 :     if (std::abs((ros::Time::now() - rampup_start_time_).toSec()) >= rampup_duration_) {
+    1504             : 
+    1505          77 :       rampup_active_ = false;
+    1506             : 
+    1507          77 :       ROS_INFO("[%s]: rampup finished", name_.c_str());
+    1508             : 
+    1509             :     } else {
+    1510             : 
+    1511         982 :       double rampup_dt = (ros::Time::now() - rampup_last_time_).toSec();
+    1512             : 
+    1513         982 :       rampup_throttle_ += double(rampup_direction_) * _rampup_speed_ * rampup_dt;
+    1514             : 
+    1515         982 :       rampup_last_time_ = ros::Time::now();
+    1516             : 
+    1517         982 :       throttle = rampup_throttle_;
+    1518             : 
+    1519         982 :       ROS_INFO_THROTTLE(0.1, "[%s]: ramping up throttle, %.4f", name_.c_str(), throttle);
+    1520             :     }
+    1521             : 
+    1522             :   } else {
+    1523             : 
+    1524       69497 :     if (desired_thrust_force >= 0) {
+    1525       69497 :       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       70556 :   bool throttle_saturated = false;
+    1534             : 
+    1535       70556 :   if (!std::isfinite(throttle)) {
+    1536             : 
+    1537           0 :     ROS_ERROR("[%s]: NaN detected in variable 'throttle'!!!", name_.c_str());
+    1538           0 :     return;
+    1539             : 
+    1540       70556 :   } else if (throttle > _throttle_saturation_) {
+    1541          28 :     throttle = _throttle_saturation_;
+    1542          28 :     ROS_WARN_THROTTLE(0.1, "[%s]: saturating throttle to %.2f", name_.c_str(), _throttle_saturation_);
+    1543       70528 :   } 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       70556 :   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       70556 :   double desired_x_accel = 0;
+    1569       70556 :   double desired_y_accel = 0;
+    1570       70556 :   double desired_z_accel = 0;
+    1571             : 
+    1572             :   {
+    1573       70556 :     Eigen::Vector3d thrust_vector = desired_thrust_force * Rd.col(2);
+    1574             : 
+    1575       70556 :     double world_accel_x = (thrust_vector(0) / total_mass) - (Iw_w_(0) / total_mass) - (Ib_w(0) / total_mass);
+    1576       70556 :     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       70556 :     double world_accel_z = tracker_command.acceleration.z;
+    1580             : 
+    1581      141112 :     geometry_msgs::Vector3Stamped world_accel;
+    1582             : 
+    1583       70556 :     world_accel.header.stamp    = ros::Time::now();
+    1584       70556 :     world_accel.header.frame_id = uav_state.header.frame_id;
+    1585       70556 :     world_accel.vector.x        = world_accel_x;
+    1586       70556 :     world_accel.vector.y        = world_accel_y;
+    1587       70556 :     world_accel.vector.z        = world_accel_z;
+    1588             : 
+    1589      211668 :     auto res = common_handlers_->transformer->transformSingle(world_accel, "fcu");
+    1590             : 
+    1591       70556 :     if (res) {
+    1592             : 
+    1593       70556 :       desired_x_accel = res.value().vector.x;
+    1594       70556 :       desired_y_accel = res.value().vector.y;
+    1595       70556 :       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       70556 :   last_control_output_.desired_orientation = mrs_lib::AttitudeConverter(Rd);
+    1603             : 
+    1604             :   // fill the unbiased desired accelerations
+    1605       70556 :   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       70556 :   last_control_output_.diagnostics.ramping_up = rampup_active_;
+    1610             : 
+    1611       70556 :   last_control_output_.diagnostics.mass_estimator  = true;
+    1612       70556 :   last_control_output_.diagnostics.mass_difference = uav_mass_difference_;
+    1613       70556 :   last_control_output_.diagnostics.total_mass      = total_mass;
+    1614             : 
+    1615       70556 :   last_control_output_.diagnostics.disturbance_estimator = true;
+    1616             : 
+    1617       70556 :   last_control_output_.diagnostics.disturbance_bx_b = -Ib_b_(0);
+    1618       70556 :   last_control_output_.diagnostics.disturbance_by_b = -Ib_b_(1);
+    1619             : 
+    1620       70556 :   last_control_output_.diagnostics.disturbance_bx_w = -Ib_w(0);
+    1621       70556 :   last_control_output_.diagnostics.disturbance_by_w = -Ib_w(1);
+    1622             : 
+    1623       70556 :   last_control_output_.diagnostics.disturbance_wx_w = -Iw_w_(0);
+    1624       70556 :   last_control_output_.diagnostics.disturbance_wy_w = -Iw_w_(1);
+    1625             : 
+    1626       70556 :   last_control_output_.diagnostics.controller_enforcing_constraints = !tracker_command.use_full_state_prediction;
+    1627             : 
+    1628       70556 :   last_control_output_.diagnostics.horizontal_speed_constraint = 0.5 * _max_speed_horizontal_;
+    1629       70556 :   last_control_output_.diagnostics.horizontal_acc_constraint   = 0.5 * _max_acceleration_horizontal_;
+    1630             : 
+    1631       70556 :   last_control_output_.diagnostics.vertical_asc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1632       70556 :   last_control_output_.diagnostics.vertical_asc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1633             : 
+    1634       70556 :   last_control_output_.diagnostics.vertical_desc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1635       70556 :   last_control_output_.diagnostics.vertical_desc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1636             : 
+    1637       70556 :   last_control_output_.diagnostics.controller = name_;
+    1638             : 
+    1639             :   // | ------------ construct the attitude reference ------------ |
+    1640             : 
+    1641       70556 :   mrs_msgs::HwApiAttitudeCmd attitude_cmd;
+    1642             : 
+    1643       70556 :   attitude_cmd.stamp       = ros::Time::now();
+    1644       70556 :   attitude_cmd.orientation = mrs_lib::AttitudeConverter(Rd);
+    1645       70556 :   attitude_cmd.throttle    = throttle;
+    1646             : 
+    1647       70556 :   if (output_modality == common::ATTITUDE) {
+    1648             : 
+    1649        1143 :     last_control_output_.control_output = attitude_cmd;
+    1650             : 
+    1651        1143 :     return;
+    1652             :   }
+    1653             : 
+    1654             :   // --------------------------------------------------------------
+    1655             :   // |                      attitude control                      |
+    1656             :   // --------------------------------------------------------------
+    1657             : 
+    1658       69413 :   Eigen::Vector3d rate_feedforward = Eigen::Vector3d::Zero(3);
+    1659             : 
+    1660       69413 :   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       69413 :   } else if (tracker_command.use_heading_rate) {
+    1665             : 
+    1666             :     // to fill in the feed forward yaw rate
+    1667       69411 :     double desired_yaw_rate = 0;
+    1668             : 
+    1669             :     try {
+    1670       69411 :       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       69411 :     rate_feedforward << 0, 0, desired_yaw_rate;
+    1677             :   }
+    1678             : 
+    1679             :   // | ------------ jerk feedforward -> angular rate ------------ |
+    1680             : 
+    1681       69413 :   Eigen::Vector3d jerk_feedforward = Eigen::Vector3d(0, 0, 0);
+    1682             : 
+    1683       69413 :   if (tracker_command.use_jerk && drs_params.jerk_feedforward) {
+    1684             : 
+    1685       49753 :     ROS_DEBUG_THROTTLE(1.0, "[%s]: using jerk feedforward", name_.c_str());
+    1686             : 
+    1687       49753 :     Eigen::Matrix3d I;
+    1688       49753 :     I << 0, 1, 0, -1, 0, 0, 0, 0, 0;
+    1689       49753 :     Eigen::Vector3d desired_jerk = Eigen::Vector3d(tracker_command.jerk.x, tracker_command.jerk.y, tracker_command.jerk.z);
+    1690       49753 :     jerk_feedforward             = (I.transpose() * Rd.transpose() * desired_jerk) / (desired_thrust_force / total_mass);
+    1691             :   }
+    1692             : 
+    1693             :   // | --------------- run the attitude controller -------------- |
+    1694             : 
+    1695       69413 :   Eigen::Vector3d attitude_rate_saturation(constraints.roll_rate, constraints.pitch_rate, constraints.yaw_rate);
+    1696             : 
+    1697       69413 :   auto attitude_rate_command = common::attitudeController(uav_state, attitude_cmd, jerk_feedforward + rate_feedforward, attitude_rate_saturation, Kq, false);
+    1698             : 
+    1699       69413 :   if (!attitude_rate_command) {
+    1700           0 :     return;
+    1701             :   }
+    1702             : 
+    1703             :   // | --------- fill in the already known attitude rate -------- |
+    1704             : 
+    1705             :   {
+    1706             :     try {
+    1707       69413 :       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       69413 :   if (output_modality == common::ATTITUDE_RATE) {
+    1716             : 
+    1717       66857 :     last_control_output_.control_output = attitude_rate_command;
+    1718             : 
+    1719       66857 :     return;
+    1720             :   }
+    1721             : 
+    1722             :   // --------------------------------------------------------------
+    1723             :   // |                    Attitude rate control                   |
+    1724             :   // --------------------------------------------------------------
+    1725             : 
+    1726        2556 :   Kw = common_handlers_->detailed_model_params->inertia.diagonal().array() * Kw;
+    1727             : 
+    1728        2556 :   auto control_group_command = common::attitudeRateController(uav_state, attitude_rate_command.value(), Kw);
+    1729             : 
+    1730        2556 :   if (!control_group_command) {
+    1731           0 :     return;
+    1732             :   }
+    1733             : 
+    1734        2556 :   if (output_modality == common::CONTROL_GROUP) {
+    1735             : 
+    1736        1254 :     last_control_output_.control_output = control_group_command;
+    1737             : 
+    1738        1254 :     return;
+    1739             :   }
+    1740             : 
+    1741             :   // --------------------------------------------------------------
+    1742             :   // |                        output mixer                        |
+    1743             :   // --------------------------------------------------------------
+    1744             : 
+    1745        2604 :   mrs_msgs::HwApiActuatorCmd actuator_cmd = common::actuatorMixer(control_group_command.value(), common_handlers_->detailed_model_params->control_group_mixer);
+    1746             : 
+    1747        1302 :   last_control_output_.control_output = actuator_cmd;
+    1748             : 
+    1749        1302 :   return;
+    1750             : }
+    1751             : 
+    1752             : //}
+    1753             : 
+    1754             : /* positionPassthrough() //{ */
+    1755             : 
+    1756         280 : void MpcController::positionPassthrough(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command) {
+    1757             : 
+    1758         280 :   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         560 :   mrs_msgs::HwApiPositionCmd cmd;
+    1764             : 
+    1765         280 :   cmd.header.frame_id = uav_state.header.frame_id;
+    1766         280 :   cmd.header.stamp    = ros::Time::now();
+    1767             : 
+    1768         280 :   cmd.position = tracker_command.position;
+    1769         280 :   cmd.heading  = tracker_command.heading;
+    1770             : 
+    1771         280 :   last_control_output_.control_output = cmd;
+    1772             : 
+    1773             :   // fill the unbiased desired accelerations
+    1774         280 :   last_control_output_.desired_unbiased_acceleration = {};
+    1775         280 :   last_control_output_.desired_orientation           = {};
+    1776         280 :   last_control_output_.desired_heading_rate          = {};
+    1777             : 
+    1778             :   // | ----------------- fill in the diagnostics ---------------- |
+    1779             : 
+    1780         280 :   last_control_output_.diagnostics.ramping_up = false;
+    1781             : 
+    1782         280 :   last_control_output_.diagnostics.mass_estimator  = false;
+    1783         280 :   last_control_output_.diagnostics.mass_difference = 0;
+    1784             : 
+    1785         280 :   last_control_output_.diagnostics.disturbance_estimator = false;
+    1786             : 
+    1787         280 :   last_control_output_.diagnostics.disturbance_bx_b = 0;
+    1788         280 :   last_control_output_.diagnostics.disturbance_by_b = 0;
+    1789             : 
+    1790         280 :   last_control_output_.diagnostics.disturbance_bx_w = 0;
+    1791         280 :   last_control_output_.diagnostics.disturbance_by_w = 0;
+    1792             : 
+    1793         280 :   last_control_output_.diagnostics.disturbance_wx_w = 0;
+    1794         280 :   last_control_output_.diagnostics.disturbance_wy_w = 0;
+    1795             : 
+    1796         280 :   last_control_output_.diagnostics.controller_enforcing_constraints = !tracker_command.use_full_state_prediction;
+    1797             : 
+    1798         280 :   last_control_output_.diagnostics.horizontal_speed_constraint = 0.5 * _max_speed_horizontal_;
+    1799         280 :   last_control_output_.diagnostics.horizontal_acc_constraint   = 0.5 * _max_acceleration_horizontal_;
+    1800             : 
+    1801         280 :   last_control_output_.diagnostics.vertical_asc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1802         280 :   last_control_output_.diagnostics.vertical_asc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1803             : 
+    1804         280 :   last_control_output_.diagnostics.vertical_desc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1805         280 :   last_control_output_.diagnostics.vertical_desc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1806             : 
+    1807         280 :   last_control_output_.diagnostics.controller = name_;
+    1808             : }
+    1809             : 
+    1810             : //}
+    1811             : 
+    1812             : /* PIDVelocityOutput() //{ */
+    1813             : 
+    1814         783 : 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         783 :   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         783 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    1823             : 
+    1824         783 :   Eigen::Vector3d pos_ref = Eigen::Vector3d(tracker_command.position.x, tracker_command.position.y, tracker_command.position.z);
+    1825         783 :   Eigen::Vector3d pos     = Eigen::Vector3d(uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z);
+    1826             : 
+    1827         783 :   double hdg_ref = tracker_command.heading;
+    1828         783 :   double hdg     = getHeadingSafely(uav_state, tracker_command);
+    1829             : 
+    1830             :   // | ------------------ velocity feedforward ------------------ |
+    1831             : 
+    1832         783 :   Eigen::Vector3d vel_ff(0, 0, 0);
+    1833             : 
+    1834         783 :   if (tracker_command.use_velocity_horizontal && tracker_command.use_velocity_vertical) {
+    1835         783 :     vel_ff = Eigen::Vector3d(tracker_command.velocity.x, tracker_command.velocity.y, tracker_command.velocity.z);
+    1836             :   }
+    1837             : 
+    1838             :   // | --------------------- control errors --------------------- |
+    1839             : 
+    1840         783 :   Eigen::Vector3d Ep = pos_ref - pos;
+    1841             : 
+    1842             :   // | --------------------------- pid -------------------------- |
+    1843             : 
+    1844         783 :   position_pid_x_.setSaturation(constraints.horizontal_speed);
+    1845         783 :   position_pid_y_.setSaturation(constraints.horizontal_speed);
+    1846         783 :   position_pid_z_.setSaturation(std::min(constraints.vertical_ascending_speed, constraints.vertical_descending_speed));
+    1847             : 
+    1848         783 :   double des_vel_x = position_pid_x_.update(Ep(0), dt);
+    1849         783 :   double des_vel_y = position_pid_y_.update(Ep(1), dt);
+    1850         783 :   double des_vel_z = position_pid_z_.update(Ep(2), dt);
+    1851             : 
+    1852             :   // | -------------------- position feedback ------------------- |
+    1853             : 
+    1854         783 :   Eigen::Vector3d des_vel = Eigen::Vector3d(des_vel_x, des_vel_y, des_vel_z) + vel_ff;
+    1855             : 
+    1856         783 :   if (control_output == common::VELOCITY_HDG) {
+    1857             : 
+    1858             :     // | --------------------- fill the output -------------------- |
+    1859             : 
+    1860         530 :     mrs_msgs::HwApiVelocityHdgCmd cmd;
+    1861             : 
+    1862         265 :     cmd.header.frame_id = uav_state.header.frame_id;
+    1863         265 :     cmd.header.stamp    = ros::Time::now();
+    1864             : 
+    1865         265 :     cmd.velocity.x = des_vel(0);
+    1866         265 :     cmd.velocity.y = des_vel(1);
+    1867         265 :     cmd.velocity.z = des_vel(2);
+    1868             : 
+    1869         265 :     cmd.heading = tracker_command.heading;
+    1870             : 
+    1871         265 :     last_control_output_.control_output = cmd;
+    1872             : 
+    1873         518 :   } else if (control_output == common::VELOCITY_HDG_RATE) {
+    1874             : 
+    1875         518 :     position_pid_heading_.setSaturation(constraints.heading_speed);
+    1876             : 
+    1877         518 :     double hdg_err = mrs_lib::geometry::sradians::diff(hdg_ref, hdg);
+    1878             : 
+    1879         518 :     double des_hdg_rate = position_pid_heading_.update(hdg_err, dt);
+    1880             : 
+    1881             :     // | --------------------------- ff --------------------------- |
+    1882             : 
+    1883         518 :     double des_hdg_ff = 0;
+    1884             : 
+    1885         518 :     if (tracker_command.use_heading_rate) {
+    1886         518 :       des_hdg_ff = tracker_command.heading_rate;
+    1887             :     }
+    1888             : 
+    1889             :     // | --------------------- fill the output -------------------- |
+    1890             : 
+    1891        1036 :     mrs_msgs::HwApiVelocityHdgRateCmd cmd;
+    1892             : 
+    1893         518 :     cmd.header.frame_id = uav_state.header.frame_id;
+    1894         518 :     cmd.header.stamp    = ros::Time::now();
+    1895             : 
+    1896         518 :     cmd.velocity.x = des_vel(0);
+    1897         518 :     cmd.velocity.y = des_vel(1);
+    1898         518 :     cmd.velocity.z = des_vel(2);
+    1899             : 
+    1900         518 :     cmd.heading_rate = des_hdg_rate + des_hdg_ff;
+    1901             : 
+    1902         518 :     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         783 :   last_control_output_.desired_unbiased_acceleration = {};
+    1911         783 :   last_control_output_.desired_orientation           = {};
+    1912         783 :   last_control_output_.desired_heading_rate          = {};
+    1913             : 
+    1914             :   // | ----------------- fill in the diagnostics ---------------- |
+    1915             : 
+    1916         783 :   last_control_output_.diagnostics.ramping_up = false;
+    1917             : 
+    1918         783 :   last_control_output_.diagnostics.mass_estimator  = false;
+    1919         783 :   last_control_output_.diagnostics.mass_difference = 0;
+    1920             : 
+    1921         783 :   last_control_output_.diagnostics.disturbance_estimator = false;
+    1922             : 
+    1923         783 :   last_control_output_.diagnostics.disturbance_bx_b = 0;
+    1924         783 :   last_control_output_.diagnostics.disturbance_by_b = 0;
+    1925             : 
+    1926         783 :   last_control_output_.diagnostics.disturbance_bx_w = 0;
+    1927         783 :   last_control_output_.diagnostics.disturbance_by_w = 0;
+    1928             : 
+    1929         783 :   last_control_output_.diagnostics.disturbance_wx_w = 0;
+    1930         783 :   last_control_output_.diagnostics.disturbance_wy_w = 0;
+    1931             : 
+    1932         783 :   last_control_output_.diagnostics.controller_enforcing_constraints = !tracker_command.use_full_state_prediction;
+    1933             : 
+    1934         783 :   last_control_output_.diagnostics.horizontal_speed_constraint = 0.5 * _max_speed_horizontal_;
+    1935         783 :   last_control_output_.diagnostics.horizontal_acc_constraint   = 0.5 * _max_acceleration_horizontal_;
+    1936             : 
+    1937         783 :   last_control_output_.diagnostics.vertical_asc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1938         783 :   last_control_output_.diagnostics.vertical_asc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1939             : 
+    1940         783 :   last_control_output_.diagnostics.vertical_desc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1941         783 :   last_control_output_.diagnostics.vertical_desc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1942             : 
+    1943         783 :   last_control_output_.diagnostics.controller = name_;
+    1944             : }
+    1945             : 
+    1946             : //}
+    1947             : 
+    1948             : // --------------------------------------------------------------
+    1949             : // |                          callbacks                         |
+    1950             : // --------------------------------------------------------------
+    1951             : 
+    1952             : /* //{ callbackDrs() */
+    1953             : 
+    1954         218 : void MpcController::callbackDrs(mrs_uav_controllers::mpc_controllerConfig &config, [[maybe_unused]] uint32_t level) {
+    1955             : 
+    1956         218 :   mrs_lib::set_mutexed(mutex_drs_params_, config, drs_params_);
+    1957             : 
+    1958         218 :   ROS_INFO("[%s]: DRS updated gains", this->name_.c_str());
+    1959         218 : }
+    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       16792 : void MpcController::timerGains(const ros::TimerEvent &event) {
+    1993             : 
+    1994       33584 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerGains", _gain_filtering_rate_, 1.0, event);
+    1995       33584 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcController::timerGains", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1996             : 
+    1997       16792 :   auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+    1998       16792 :   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       16792 :   bool   bypass_filter = (mute_gains_ || mute_gains_by_tracker_);
+    2003       16792 :   double gain_coeff    = (mute_gains_ || mute_gains_by_tracker_) ? _gain_mute_coefficient_ : 1.0;
+    2004             : 
+    2005       16792 :   mute_gains_ = false;
+    2006             : 
+    2007       16792 :   double dt = (event.current_real - event.last_real).toSec();
+    2008             : 
+    2009       16792 :   if (!std::isfinite(dt) || (dt <= 0) || (dt > 5 * (1.0 / _gain_filtering_rate_))) {
+    2010         193 :     return;
+    2011             :   }
+    2012             : 
+    2013       16599 :   bool updated = false;
+    2014             : 
+    2015       16599 :   gains.kq_roll_pitch = calculateGainChange(dt, gains.kq_roll_pitch, drs_params.kq_roll_pitch * gain_coeff, bypass_filter, "kq_roll_pitch", updated);
+    2016       16599 :   gains.kq_yaw        = calculateGainChange(dt, gains.kq_yaw, drs_params.kq_yaw * gain_coeff, bypass_filter, "kq_yaw", updated);
+    2017       16599 :   gains.km            = calculateGainChange(dt, gains.km, drs_params.km * gain_coeff, bypass_filter, "km", updated);
+    2018       16599 :   gains.kiwxy         = calculateGainChange(dt, gains.kiwxy, drs_params.kiwxy * gain_coeff, bypass_filter, "kiwxy", updated);
+    2019       16599 :   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       16599 :   gains.kiwxy_lim = calculateGainChange(dt, gains.kiwxy_lim, drs_params.kiwxy_lim, false, "kiwxy_lim", updated);
+    2023       16599 :   gains.kibxy_lim = calculateGainChange(dt, gains.kibxy_lim, drs_params.kibxy_lim, false, "kibxy_lim", updated);
+    2024       16599 :   gains.km_lim    = calculateGainChange(dt, gains.km_lim, drs_params.km_lim, false, "km_lim", updated);
+    2025             : 
+    2026       16599 :   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       16599 :   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      132792 : 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      132792 :   double change = desired_value - current_value;
+    2059             : 
+    2060      132792 :   double gains_filter_max_change = _gains_filter_change_rate_ * dt;
+    2061      132792 :   double gains_filter_min_change = _gains_filter_min_change_rate_ * dt;
+    2062             : 
+    2063      132792 :   if (!bypass_rate) {
+    2064             : 
+    2065             :     // if current value is near 0...
+    2066             :     double change_in_perc;
+    2067             :     double saturated_change;
+    2068             : 
+    2069      132792 :     if (std::abs(current_value) < 1e-6) {
+    2070           0 :       change *= gains_filter_max_change;
+    2071             :     } else {
+    2072             : 
+    2073      132792 :       saturated_change = change;
+    2074             : 
+    2075      132792 :       change_in_perc = ((current_value + saturated_change) / current_value) - 1.0;
+    2076             : 
+    2077      132792 :       if (change_in_perc > gains_filter_max_change) {
+    2078           0 :         saturated_change = current_value * gains_filter_max_change;
+    2079      132792 :       } else if (change_in_perc < -gains_filter_max_change) {
+    2080           0 :         saturated_change = current_value * -gains_filter_max_change;
+    2081             :       }
+    2082             : 
+    2083      132792 :       if (std::abs(saturated_change) < std::abs(change) * gains_filter_min_change) {
+    2084           0 :         change *= gains_filter_min_change;
+    2085             :       } else {
+    2086      132792 :         change = saturated_change;
+    2087             :       }
+    2088             :     }
+    2089             :   }
+    2090             : 
+    2091      132792 :   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      132792 :   return current_value + change;
+    2097             : }
+    2098             : 
+    2099             : //}
+    2100             : 
+    2101             : /* getHeadingSafely() //{ */
+    2102             : 
+    2103       72406 : double MpcController::getHeadingSafely(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command) {
+    2104             : 
+    2105             :   try {
+    2106       72406 :     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         109 : 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-12-17 22:35:29Functions: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()23
mrs_uav_controllers::se3_controller::Se3Controller::activate(mrs_uav_managers::Controller::ControlOutput const&)27
(anonymous namespace)::ProxyExec0::ProxyExec0()109
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>)109
mrs_uav_controllers::se3_controller::Se3Controller::callbackDrs(mrs_uav_controllers::se3_controllerConfig&, unsigned int)219
mrs_uav_controllers::se3_controller::Se3Controller::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)225
mrs_uav_controllers::se3_controller::Se3Controller::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)431
mrs_uav_controllers::se3_controller::Se3Controller::getStatus()2892
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&)3109
mrs_uav_controllers::se3_controller::Se3Controller::timerGains(ros::TimerEvent const&)4711
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&)25122
mrs_uav_controllers::se3_controller::Se3Controller::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)28231
mrs_uav_controllers::se3_controller::Se3Controller::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)28456
mrs_uav_controllers::se3_controller::Se3Controller::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)65576
mrs_uav_controllers::se3_controller::Se3Controller::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)123369
+
+
+ + + +
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..d17566df3e --- /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-12-17 22:35:29Functions:151788.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()109
mrs_uav_controllers::se3_controller::Se3Controller::deactivate()23
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>)109
mrs_uav_controllers::se3_controller::Se3Controller::timerGains(ros::TimerEvent const&)4711
mrs_uav_controllers::se3_controller::Se3Controller::callbackDrs(mrs_uav_controllers::se3_controllerConfig&, unsigned int)219
mrs_uav_controllers::se3_controller::Se3Controller::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)28456
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&)25122
mrs_uav_controllers::se3_controller::Se3Controller::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)431
mrs_uav_controllers::se3_controller::Se3Controller::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)123369
mrs_uav_controllers::se3_controller::Se3Controller::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)28231
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&)3109
mrs_uav_controllers::se3_controller::Se3Controller::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)65576
mrs_uav_controllers::se3_controller::Se3Controller::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)225
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()2892
+
+
+ + + +
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..f0ce5d330f --- /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-12-17 22:35:29Functions: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         109 : 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         109 :   nh_ = nh;
+     217             : 
+     218         109 :   common_handlers_  = common_handlers;
+     219         109 :   private_handlers_ = private_handlers;
+     220             : 
+     221         109 :   _uav_mass_ = common_handlers->getMass();
+     222             : 
+     223         109 :   ros::Time::waitForValid();
+     224             : 
+     225             :   // | ---------- loading params using the parent's nh ---------- |
+     226             : 
+     227         218 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     228             : 
+     229         109 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     230             : 
+     231         109 :   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         109 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/se3_controller.yaml");
+     239         109 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/se3_controller.yaml");
+     240             : 
+     241         218 :   const std::string yaml_namespace = "mrs_uav_controllers/se3_controller/";
+     242             : 
+     243             :   // lateral gains
+     244         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kp", gains_.kpxy);
+     245         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kv", gains_.kvxy);
+     246         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/ka", gains_.kaxy);
+     247             : 
+     248         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kiw", gains_.kiwxy);
+     249         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kib", gains_.kibxy);
+     250             : 
+     251             :   // | ------------------------- rampup ------------------------- |
+     252             : 
+     253         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/rampup/enabled", _rampup_enabled_);
+     254         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/rampup/speed", _rampup_speed_);
+     255             : 
+     256             :   // height gains
+     257         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/vertical/kp", gains_.kpz);
+     258         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/vertical/kv", gains_.kvz);
+     259         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/vertical/ka", gains_.kaz);
+     260             : 
+     261             :   // attitude gains
+     262         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/attitude/kq_roll_pitch", gains_.kq_roll_pitch);
+     263         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/attitude/kq_yaw", gains_.kq_yaw);
+     264             : 
+     265             :   // attitude rate gains
+     266         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/attitude_rate_gains/kw_roll_pitch", gains_.kw_roll_pitch);
+     267         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/attitude_rate_gains/kw_yaw", gains_.kw_yaw);
+     268             : 
+     269             :   // mass estimator
+     270         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/mass_estimator/km", gains_.km);
+     271         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/mass_estimator/km_lim", gains_.km_lim);
+     272             : 
+     273             :   // integrator limits
+     274         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kiw_lim", gains_.kiwxy_lim);
+     275         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kib_lim", gains_.kibxy_lim);
+     276             : 
+     277             :   // constraints
+     278         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/constraints/tilt_angle_failsafe/enabled", _tilt_angle_failsafe_enabled_);
+     279         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/constraints/tilt_angle_failsafe/limit", _tilt_angle_failsafe_);
+     280             : 
+     281         109 :   _tilt_angle_failsafe_ = M_PI * (_tilt_angle_failsafe_ / 180.0);
+     282             : 
+     283         109 :   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         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/constraints/throttle_saturation", _throttle_saturation_);
+     289             : 
+     290             :   // gain filtering
+     291         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/gain_filtering/perc_change_rate", _gains_filter_change_rate_);
+     292         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/gain_filtering/min_change_rate", _gains_filter_min_change_rate_);
+     293         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/gain_filtering/rate", _gain_filtering_rate_);
+     294         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/gain_filtering/gain_mute_coefficient", _gain_mute_coefficient_);
+     295             : 
+     296             :   // output mode
+     297         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/preferred_output", drs_params_.preferred_output_mode);
+     298             : 
+     299         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/rotation_matrix", drs_params_.rotation_type);
+     300             : 
+     301             :   // angular rate feed forward
+     302         218 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/angular_rate_feedforward/parasitic_pitch_roll",
+     303         109 :                                             drs_params_.pitch_roll_heading_rate_compensation);
+     304         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/angular_rate_feedforward/jerk", drs_params_.jerk_feedforward);
+     305             : 
+     306             :   // | ------------------- position pid params ------------------ |
+     307             : 
+     308         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/p", _pos_pid_p_);
+     309         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/i", _pos_pid_i_);
+     310         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/d", _pos_pid_d_);
+     311             : 
+     312         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/p", _hdg_pid_p_);
+     313         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/i", _hdg_pid_i_);
+     314         109 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/d", _hdg_pid_d_);
+     315             : 
+     316             :   // | ------------------ finish loading params ----------------- |
+     317             : 
+     318         109 :   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         109 :   if (!(drs_params_.preferred_output_mode == OUTPUT_ACTUATORS || drs_params_.preferred_output_mode == OUTPUT_CONTROL_GROUP ||
+     326         109 :         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         109 :   uav_mass_difference_ = 0;
+     333         109 :   Iw_w_                = Eigen::Vector2d::Zero(2);
+     334         109 :   Ib_b_                = Eigen::Vector2d::Zero(2);
+     335             : 
+     336             :   // | --------------- dynamic reconfigure server --------------- |
+     337             : 
+     338         109 :   drs_params_.kpxy             = gains_.kpxy;
+     339         109 :   drs_params_.kvxy             = gains_.kvxy;
+     340         109 :   drs_params_.kaxy             = gains_.kaxy;
+     341         109 :   drs_params_.kiwxy            = gains_.kiwxy;
+     342         109 :   drs_params_.kibxy            = gains_.kibxy;
+     343         109 :   drs_params_.kpz              = gains_.kpz;
+     344         109 :   drs_params_.kvz              = gains_.kvz;
+     345         109 :   drs_params_.kaz              = gains_.kaz;
+     346         109 :   drs_params_.kq_roll_pitch    = gains_.kq_roll_pitch;
+     347         109 :   drs_params_.kq_yaw           = gains_.kq_yaw;
+     348         109 :   drs_params_.kiwxy_lim        = gains_.kiwxy_lim;
+     349         109 :   drs_params_.kibxy_lim        = gains_.kibxy_lim;
+     350         109 :   drs_params_.km               = gains_.km;
+     351         109 :   drs_params_.km_lim           = gains_.km_lim;
+     352         109 :   drs_params_.jerk_feedforward = true;
+     353             : 
+     354         109 :   drs_.reset(new Drs_t(mutex_drs_, nh_));
+     355         109 :   drs_->updateConfig(drs_params_);
+     356         109 :   Drs_t::CallbackType f = boost::bind(&Se3Controller::callbackDrs, this, _1, _2);
+     357         109 :   drs_->setCallback(f);
+     358             : 
+     359             :   // | ------------------------- timers ------------------------- |
+     360             : 
+     361         109 :   timer_gains_ = nh_.createTimer(ros::Rate(_gain_filtering_rate_), &Se3Controller::timerGains, this, false, false);
+     362             : 
+     363             :   // | ---------------------- position pid ---------------------- |
+     364             : 
+     365         109 :   position_pid_x_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     366         109 :   position_pid_y_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     367         109 :   position_pid_z_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     368         109 :   position_pid_heading_.setParams(_hdg_pid_p_, _hdg_pid_d_, _hdg_pid_i_, -1, 0.1);
+     369             : 
+     370             :   // | ------------------------ profiler ------------------------ |
+     371             : 
+     372         109 :   profiler_ = mrs_lib::Profiler(common_handlers_->parent_nh, "Se3Controller", _profiler_enabled_);
+     373             : 
+     374             :   // | ----------------------- finish init ---------------------- |
+     375             : 
+     376         109 :   ROS_INFO("[Se3Controller]: initialized");
+     377             : 
+     378         109 :   is_initialized_ = true;
+     379             : 
+     380         109 :   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          23 : void Se3Controller::deactivate(void) {
+     461             : 
+     462          23 :   is_active_           = false;
+     463          23 :   first_iteration_     = false;
+     464          23 :   uav_mass_difference_ = 0;
+     465             : 
+     466          23 :   timer_gains_.stop();
+     467             : 
+     468          23 :   ROS_INFO("[Se3Controller]: deactivated");
+     469          23 : }
+     470             : 
+     471             : //}
+     472             : 
+     473             : /* updateInactive() //{ */
+     474             : 
+     475      123369 : void Se3Controller::updateInactive(const mrs_msgs::UavState& uav_state, [[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand>& tracker_command) {
+     476             : 
+     477      123369 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     478             : 
+     479      123369 :   last_update_time_ = uav_state.header.stamp;
+     480             : 
+     481      123369 :   first_iteration_ = false;
+     482      123369 : }
+     483             : 
+     484             : //}
+     485             : 
+     486             : /* //{ updateWhenActive() */
+     487             : 
+     488       28456 : Se3Controller::ControlOutput Se3Controller::updateActive(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command) {
+     489             : 
+     490       85368 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("updateActive");
+     491       85368 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("Se3Controller::updateActive", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     492             : 
+     493       56912 :   auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+     494             : 
+     495       28456 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     496             : 
+     497       28456 :   last_control_output_.desired_heading_rate          = {};
+     498       28456 :   last_control_output_.desired_orientation           = {};
+     499       28456 :   last_control_output_.desired_unbiased_acceleration = {};
+     500       28456 :   last_control_output_.control_output                = {};
+     501             : 
+     502             :   // | -------------------- calculate the dt -------------------- |
+     503             : 
+     504             :   double dt;
+     505             : 
+     506       28456 :   if (first_iteration_) {
+     507          27 :     dt               = 0.01;
+     508          27 :     first_iteration_ = false;
+     509             :   } else {
+     510       28429 :     dt = (uav_state.header.stamp - last_update_time_).toSec();
+     511             :   }
+     512             : 
+     513       28456 :   last_update_time_ = uav_state.header.stamp;
+     514             : 
+     515       28456 :   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       28456 :   auto lowest_modality = common::getLowestOuput(common_handlers_->control_output_modalities);
+     525             : 
+     526       28456 :   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       28456 :   if (drs_params.preferred_output_mode == OUTPUT_ATTITUDE_RATE && common_handlers_->control_output_modalities.attitude_rate) {
+     536       20170 :     ROS_DEBUG_THROTTLE(1.0, "[Se3Controller]: prioritizing attitude rate output");
+     537       20170 :     lowest_modality = common::ATTITUDE_RATE;
+     538        8286 :   } 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        8286 :   } 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        8286 :   } 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       28456 :   switch (lowest_modality.value()) {
+     550             : 
+     551         225 :     case common::POSITION: {
+     552         225 :       positionPassthrough(uav_state, tracker_command);
+     553         225 :       break;
+     554             :     }
+     555             : 
+     556        1168 :     case common::VELOCITY_HDG: {
+     557        1168 :       PIDVelocityOutput(uav_state, tracker_command, common::VELOCITY_HDG, dt);
+     558        1168 :       break;
+     559             :     }
+     560             : 
+     561        1941 :     case common::VELOCITY_HDG_RATE: {
+     562        1941 :       PIDVelocityOutput(uav_state, tracker_command, common::VELOCITY_HDG_RATE, dt);
+     563        1941 :       break;
+     564             :     }
+     565             : 
+     566         573 :     case common::ACCELERATION_HDG: {
+     567         573 :       SE3Controller(uav_state, tracker_command, dt, common::ACCELERATION_HDG);
+     568         573 :       break;
+     569             :     }
+     570             : 
+     571         571 :     case common::ACCELERATION_HDG_RATE: {
+     572         571 :       SE3Controller(uav_state, tracker_command, dt, common::ACCELERATION_HDG_RATE);
+     573         571 :       break;
+     574             :     }
+     575             : 
+     576        1199 :     case common::ATTITUDE: {
+     577        1199 :       SE3Controller(uav_state, tracker_command, dt, common::ATTITUDE);
+     578        1199 :       break;
+     579             :     }
+     580             : 
+     581       20170 :     case common::ATTITUDE_RATE: {
+     582       20170 :       SE3Controller(uav_state, tracker_command, dt, common::ATTITUDE_RATE);
+     583       20170 :       break;
+     584             :     }
+     585             : 
+     586        1307 :     case common::CONTROL_GROUP: {
+     587        1307 :       SE3Controller(uav_state, tracker_command, dt, common::CONTROL_GROUP);
+     588        1307 :       break;
+     589             :     }
+     590             : 
+     591        1302 :     case common::ACTUATORS_CMD: {
+     592        1302 :       SE3Controller(uav_state, tracker_command, dt, common::ACTUATORS_CMD);
+     593        1302 :       break;
+     594             :     }
+     595             : 
+     596       28456 :     default: {
+     597             :     }
+     598             :   }
+     599             : 
+     600       28456 :   return last_control_output_;
+     601             : }
+     602             : 
+     603             : //}
+     604             : 
+     605             : /* //{ getStatus() */
+     606             : 
+     607        2892 : const mrs_msgs::ControllerStatus Se3Controller::getStatus() {
+     608             : 
+     609        2892 :   mrs_msgs::ControllerStatus controller_status;
+     610             : 
+     611        2892 :   controller_status.active = is_active_;
+     612             : 
+     613        2892 :   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         431 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr Se3Controller::setConstraints([
+     674             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& constraints) {
+     675             : 
+     676         431 :   if (!is_initialized_) {
+     677           0 :     return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+     678             :   }
+     679             : 
+     680         431 :   mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_);
+     681             : 
+     682         431 :   ROS_INFO("[Se3Controller]: updating constraints");
+     683             : 
+     684         862 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     685         431 :   res.success = true;
+     686         431 :   res.message = "constraints updated";
+     687             : 
+     688         431 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     689             : }
+     690             : 
+     691             : //}
+     692             : 
+     693             : // --------------------------------------------------------------
+     694             : // |                         controllers                        |
+     695             : // --------------------------------------------------------------
+     696             : 
+     697             : /* SE3Controller() //{ */
+     698             : 
+     699       25122 : 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       50244 :   auto drs_params  = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+     703       25122 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     704       25122 :   auto gains       = mrs_lib::get_mutexed(mutex_gains_, gains_);
+     705             : 
+     706             :   // | ----------------- get the current heading ---------------- |
+     707             : 
+     708       25122 :   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       25122 :   Eigen::Vector3d Rp = Eigen::Vector3d::Zero(3);
+     719       25122 :   Eigen::Vector3d Rv = Eigen::Vector3d::Zero(3);
+     720       25122 :   Eigen::Vector3d Ra = Eigen::Vector3d::Zero(3);
+     721             : 
+     722       25122 :   if (tracker_command.use_position_vertical || tracker_command.use_position_horizontal) {
+     723             : 
+     724       25122 :     if (tracker_command.use_position_horizontal) {
+     725       25122 :       Rp(0) = tracker_command.position.x;
+     726       25122 :       Rp(1) = tracker_command.position.y;
+     727             :     } else {
+     728           0 :       Rv(0) = 0;
+     729           0 :       Rv(1) = 0;
+     730             :     }
+     731             : 
+     732       25122 :     if (tracker_command.use_position_vertical) {
+     733       25122 :       Rp(2) = tracker_command.position.z;
+     734             :     } else {
+     735           0 :       Rv(2) = 0;
+     736             :     }
+     737             :   }
+     738             : 
+     739       25122 :   if (tracker_command.use_velocity_horizontal) {
+     740       25122 :     Rv(0) = tracker_command.velocity.x;
+     741       25122 :     Rv(1) = tracker_command.velocity.y;
+     742             :   } else {
+     743           0 :     Rv(0) = 0;
+     744           0 :     Rv(1) = 0;
+     745             :   }
+     746             : 
+     747       25122 :   if (tracker_command.use_velocity_vertical) {
+     748       25122 :     Rv(2) = tracker_command.velocity.z;
+     749             :   } else {
+     750           0 :     Rv(2) = 0;
+     751             :   }
+     752             : 
+     753       25122 :   if (tracker_command.use_acceleration) {
+     754       25119 :     Ra << tracker_command.acceleration.x, tracker_command.acceleration.y, tracker_command.acceleration.z;
+     755             :   } else {
+     756           3 :     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       25122 :   Eigen::Vector3d Op(uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z);
+     764       25122 :   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       25122 :   Eigen::Matrix3d R = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+     768             : 
+     769             :   // Ow - UAV angular rate
+     770       25122 :   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       25122 :   Eigen::Vector3d Ep(0, 0, 0);
+     776             : 
+     777       25122 :   if (tracker_command.use_position_horizontal || tracker_command.use_position_vertical) {
+     778       25122 :     Ep = Rp - Op;
+     779             :   }
+     780             : 
+     781             :   // velocity control error
+     782       25122 :   Eigen::Vector3d Ev(0, 0, 0);
+     783             : 
+     784       25122 :   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       25122 :     Ev = Rv - Ov;
+     787             :   }
+     788             : 
+     789             :   // | --------------------- load the gains --------------------- |
+     790             : 
+     791       25122 :   mute_gains_by_tracker_ = tracker_command.disable_position_gains;
+     792             : 
+     793       25122 :   Eigen::Vector3d Ka(0, 0, 0);
+     794       25122 :   Eigen::Array3d  Kp(0, 0, 0);
+     795       25122 :   Eigen::Array3d  Kv(0, 0, 0);
+     796       25122 :   Eigen::Array3d  Kq(0, 0, 0);
+     797       25122 :   Eigen::Array3d  Kw(0, 0, 0);
+     798             : 
+     799             :   {
+     800       25122 :     std::scoped_lock lock(mutex_gains_);
+     801             : 
+     802       25122 :     if (tracker_command.use_position_horizontal) {
+     803       25122 :       Kp(0) = gains.kpxy;
+     804       25122 :       Kp(1) = gains.kpxy;
+     805             :     } else {
+     806           0 :       Kp(0) = 0;
+     807           0 :       Kp(1) = 0;
+     808             :     }
+     809             : 
+     810       25122 :     if (tracker_command.use_position_vertical) {
+     811       25122 :       Kp(2) = gains.kpz;
+     812             :     } else {
+     813           0 :       Kp(2) = 0;
+     814             :     }
+     815             : 
+     816       25122 :     if (tracker_command.use_velocity_horizontal) {
+     817       25122 :       Kv(0) = gains.kvxy;
+     818       25122 :       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       25122 :     if (tracker_command.use_velocity_vertical || tracker_command.use_position_vertical) {
+     826       25122 :       Kv(2) = gains.kvz;
+     827             :     } else {
+     828           0 :       Kv(2) = 0;
+     829             :     }
+     830             : 
+     831       25122 :     if (tracker_command.use_acceleration) {
+     832       25119 :       Ka << gains.kaxy, gains.kaxy, gains.kaz;
+     833             :     } else {
+     834           3 :       Ka << 0, 0, 0;
+     835             :     }
+     836             : 
+     837       25122 :     if (!tracker_command.use_attitude_rate) {
+     838       25122 :       Kq << gains.kq_roll_pitch, gains.kq_roll_pitch, gains.kq_yaw;
+     839             :     }
+     840             : 
+     841       25122 :     Kw(0) = gains.kw_roll_pitch;
+     842       25122 :     Kw(1) = gains.kw_roll_pitch;
+     843       25122 :     Kw(2) = gains.kw_yaw;
+     844             :   }
+     845             : 
+     846       25122 :   Kp = Kp * (_uav_mass_ + uav_mass_difference_);
+     847       25122 :   Kv = Kv * (_uav_mass_ + uav_mass_difference_);
+     848             : 
+     849             :   // | --------------- desired orientation matrix --------------- |
+     850             : 
+     851             :   // get body integral in the world frame
+     852             : 
+     853       25122 :   Eigen::Vector2d Ib_w = Eigen::Vector2d(0, 0);
+     854             : 
+     855             :   {
+     856       50244 :     geometry_msgs::Vector3Stamped Ib_b_stamped;
+     857             : 
+     858       25122 :     Ib_b_stamped.header.stamp    = ros::Time::now();
+     859       25122 :     Ib_b_stamped.header.frame_id = "fcu_untilted";
+     860       25122 :     Ib_b_stamped.vector.x        = Ib_b_(0);
+     861       25122 :     Ib_b_stamped.vector.y        = Ib_b_(1);
+     862       25122 :     Ib_b_stamped.vector.z        = 0;
+     863             : 
+     864       50244 :     auto res = common_handlers_->transformer->transformSingle(Ib_b_stamped, uav_state_.header.frame_id);
+     865             : 
+     866       25122 :     if (res) {
+     867       25122 :       Ib_w(0) = res.value().vector.x;
+     868       25122 :       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       25122 :   double total_mass = _uav_mass_ + uav_mass_difference_;
+     877             : 
+     878       25122 :   Eigen::Vector3d feed_forward      = total_mass * (Eigen::Vector3d(0, 0, common_handlers_->g) + Ra);
+     879       25122 :   Eigen::Vector3d position_feedback = Kp * Ep.array();
+     880       25122 :   Eigen::Vector3d velocity_feedback = Kv * Ev.array();
+     881       25122 :   Eigen::Vector3d integral_feedback;
+     882             :   {
+     883       25122 :     std::scoped_lock lock(mutex_integrals_);
+     884             : 
+     885       25122 :     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       50244 :     std::scoped_lock lock(mutex_gains_, mutex_integrals_);
+     900             : 
+     901       25122 :     Eigen::Vector3d integration_switch(1, 1, 0);
+     902             : 
+     903             :     // integrate the world error
+     904       25122 :     if (tracker_command.use_position_horizontal) {
+     905       25122 :       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       25122 :     bool world_integral_saturated = false;
+     912       25122 :     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       25122 :     } else if (Iw_w_(0) > gains.kiwxy_lim) {
+     916           0 :       Iw_w_(0)                 = gains.kiwxy_lim;
+     917           0 :       world_integral_saturated = true;
+     918       25122 :     } 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       25122 :     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       25122 :     world_integral_saturated = false;
+     929       25122 :     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       25122 :     } else if (Iw_w_(1) > gains.kiwxy_lim) {
+     933           0 :       Iw_w_(1)                 = gains.kiwxy_lim;
+     934           0 :       world_integral_saturated = true;
+     935       25122 :     } 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       25122 :     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       50244 :     std::scoped_lock lock(mutex_gains_);
+     955             : 
+     956       25122 :     Eigen::Vector2d Ep_fcu_untilted = Eigen::Vector2d(0, 0);  // position error in the untilted frame of the UAV
+     957       25122 :     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       50244 :       geometry_msgs::Vector3Stamped Ep_stamped;
+     963             : 
+     964       25122 :       Ep_stamped.header.stamp    = ros::Time::now();
+     965       25122 :       Ep_stamped.header.frame_id = uav_state_.header.frame_id;
+     966       25122 :       Ep_stamped.vector.x        = Ep(0);
+     967       25122 :       Ep_stamped.vector.y        = Ep(1);
+     968       25122 :       Ep_stamped.vector.z        = Ep(2);
+     969             : 
+     970       75366 :       auto res = common_handlers_->transformer->transformSingle(Ep_stamped, "fcu_untilted");
+     971             : 
+     972       25122 :       if (res) {
+     973       25122 :         Ep_fcu_untilted(0) = res.value().vector.x;
+     974       25122 :         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       50244 :       geometry_msgs::Vector3Stamped Ev_stamped;
+     983             : 
+     984       25122 :       Ev_stamped.header.stamp    = ros::Time::now();
+     985       25122 :       Ev_stamped.header.frame_id = uav_state_.header.frame_id;
+     986       25122 :       Ev_stamped.vector.x        = Ev(0);
+     987       25122 :       Ev_stamped.vector.y        = Ev(1);
+     988       25122 :       Ev_stamped.vector.z        = Ev(2);
+     989             : 
+     990       75366 :       auto res = common_handlers_->transformer->transformSingle(Ev_stamped, "fcu_untilted");
+     991             : 
+     992       25122 :       if (res) {
+     993       25122 :         Ev_fcu_untilted(0) = res.value().vector.x;
+     994       25122 :         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       25122 :     if (tracker_command.use_position_horizontal) {
+    1002       25122 :       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       25122 :     bool body_integral_saturated = false;
+    1009       25122 :     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       25122 :     } else if (Ib_b_(0) > gains.kibxy_lim) {
+    1013           0 :       Ib_b_(0)                = gains.kibxy_lim;
+    1014           0 :       body_integral_saturated = true;
+    1015       25122 :     } 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       25122 :     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       25122 :     body_integral_saturated = false;
+    1026       25122 :     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       25122 :     } else if (Ib_b_(1) > gains.kibxy_lim) {
+    1030           0 :       Ib_b_(1)                = gains.kibxy_lim;
+    1031           0 :       body_integral_saturated = true;
+    1032       25122 :     } 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       25122 :     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       25122 :   if (output_modality == common::ACCELERATION_HDG || output_modality == common::ACCELERATION_HDG_RATE) {
+    1045             : 
+    1046        1144 :     Eigen::Vector3d des_acc = (position_feedback + velocity_feedback + integral_feedback) / total_mass + Ra;
+    1047             : 
+    1048        1144 :     if (output_modality == common::ACCELERATION_HDG) {
+    1049             : 
+    1050        1146 :       mrs_msgs::HwApiAccelerationHdgCmd cmd;
+    1051             : 
+    1052         573 :       cmd.acceleration.x = des_acc(0);
+    1053         573 :       cmd.acceleration.y = des_acc(1);
+    1054         573 :       cmd.acceleration.z = des_acc(2);
+    1055             : 
+    1056         573 :       cmd.heading = tracker_command.heading;
+    1057             : 
+    1058         573 :       last_control_output_.control_output = cmd;
+    1059             : 
+    1060             :     } else {
+    1061             : 
+    1062         571 :       double des_hdg_ff = 0;
+    1063             : 
+    1064         571 :       if (tracker_command.use_heading_rate) {
+    1065         571 :         des_hdg_ff = tracker_command.heading_rate;
+    1066             :       }
+    1067             : 
+    1068        1142 :       mrs_msgs::HwApiAccelerationHdgRateCmd cmd;
+    1069             : 
+    1070         571 :       cmd.acceleration.x = des_acc(0);
+    1071         571 :       cmd.acceleration.y = des_acc(1);
+    1072         571 :       cmd.acceleration.z = des_acc(2);
+    1073             : 
+    1074         571 :       position_pid_heading_.setSaturation(constraints.heading_speed);
+    1075             : 
+    1076         571 :       double hdg_err = mrs_lib::geometry::sradians::diff(tracker_command.heading, uav_heading);
+    1077             : 
+    1078         571 :       double des_hdg_rate = position_pid_heading_.update(hdg_err, dt) + des_hdg_ff;
+    1079             : 
+    1080         571 :       cmd.heading_rate = des_hdg_rate;
+    1081             : 
+    1082         571 :       last_control_output_.desired_heading_rate = des_hdg_rate;
+    1083             : 
+    1084         571 :       last_control_output_.control_output = cmd;
+    1085             :     }
+    1086             : 
+    1087             :     // | -------------- unbiased desired acceleration ------------- |
+    1088             : 
+    1089        1144 :     Eigen::Vector3d unbiased_des_acc(0, 0, 0);
+    1090             : 
+    1091             :     {
+    1092             : 
+    1093        1144 :       Eigen::Vector3d unbiased_des_acc_world = (position_feedback + velocity_feedback) / total_mass + Ra;
+    1094             : 
+    1095        2288 :       geometry_msgs::Vector3Stamped world_accel;
+    1096             : 
+    1097        1144 :       world_accel.header.stamp    = ros::Time::now();
+    1098        1144 :       world_accel.header.frame_id = uav_state.header.frame_id;
+    1099        1144 :       world_accel.vector.x        = unbiased_des_acc_world(0);
+    1100        1144 :       world_accel.vector.y        = unbiased_des_acc_world(1);
+    1101        1144 :       world_accel.vector.z        = unbiased_des_acc_world(2);
+    1102             : 
+    1103        3432 :       auto res = common_handlers_->transformer->transformSingle(world_accel, "fcu");
+    1104             : 
+    1105        1144 :       if (res) {
+    1106        1144 :         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        1144 :     last_control_output_.desired_unbiased_acceleration = unbiased_des_acc;
+    1112             : 
+    1113             :     // | ----------------- fill in the diagnostics ---------------- |
+    1114             : 
+    1115        1144 :     last_control_output_.diagnostics.ramping_up = false;
+    1116             : 
+    1117        1144 :     last_control_output_.diagnostics.mass_estimator  = false;
+    1118        1144 :     last_control_output_.diagnostics.mass_difference = 0;
+    1119        1144 :     last_control_output_.diagnostics.total_mass      = total_mass;
+    1120             : 
+    1121        1144 :     last_control_output_.diagnostics.disturbance_estimator = true;
+    1122             : 
+    1123        1144 :     last_control_output_.diagnostics.disturbance_bx_b = -Ib_b_(0);
+    1124        1144 :     last_control_output_.diagnostics.disturbance_by_b = -Ib_b_(1);
+    1125             : 
+    1126        1144 :     last_control_output_.diagnostics.disturbance_bx_w = -Ib_w(0);
+    1127        1144 :     last_control_output_.diagnostics.disturbance_by_w = -Ib_w(1);
+    1128             : 
+    1129        1144 :     last_control_output_.diagnostics.disturbance_wx_w = -Iw_w_(0);
+    1130        1144 :     last_control_output_.diagnostics.disturbance_wy_w = -Iw_w_(1);
+    1131             : 
+    1132        1144 :     last_control_output_.diagnostics.controller_enforcing_constraints = false;
+    1133             : 
+    1134        1144 :     last_control_output_.diagnostics.controller = "Se3Controller";
+    1135             : 
+    1136        1144 :     return;
+    1137             :   }
+    1138             : 
+    1139             :   /* mass estimatior //{ */
+    1140             : 
+    1141             :   // --------------------------------------------------------------
+    1142             :   // |                integrate the mass difference               |
+    1143             :   // --------------------------------------------------------------
+    1144             : 
+    1145             :   {
+    1146       47956 :     std::scoped_lock lock(mutex_gains_);
+    1147             : 
+    1148       23978 :     if (tracker_command.use_position_vertical && !rampup_active_) {
+    1149       23963 :       uav_mass_difference_ += gains.km * Ep(2) * dt;
+    1150             :     }
+    1151             : 
+    1152             :     // saturate the mass estimator
+    1153       23978 :     bool uav_mass_saturated = false;
+    1154       23978 :     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       23978 :     } else if (uav_mass_difference_ > gains.km_lim) {
+    1158           0 :       uav_mass_difference_ = gains.km_lim;
+    1159           0 :       uav_mass_saturated   = true;
+    1160       23978 :     } 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       23978 :     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       23978 :   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       23978 :   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       23978 :   double tilt_safety_limit = _tilt_angle_failsafe_enabled_ ? _tilt_angle_failsafe_ : std::numeric_limits<double>::max();
+    1188             : 
+    1189       23978 :   auto f_normed_sanitized = common::sanitizeDesiredForce(f.normalized(), tilt_safety_limit, constraints.tilt, "Se3Controller");
+    1190             : 
+    1191       23978 :   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       23978 :   Eigen::Vector3d f_normed = f_normed_sanitized.value();
+    1205             : 
+    1206             :   // --------------------------------------------------------------
+    1207             :   // |               desired orientation + throttle               |
+    1208             :   // --------------------------------------------------------------
+    1209             : 
+    1210             :   // | ------------------- desired orientation ------------------ |
+    1211             : 
+    1212       23978 :   Eigen::Matrix3d Rd;
+    1213             : 
+    1214       23978 :   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       23978 :     Eigen::Vector3d bxd;  // desired heading vector
+    1231             : 
+    1232       23978 :     if (tracker_command.use_heading) {
+    1233       23978 :       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       23978 :     Rd = common::so3transform(f_normed, bxd, drs_params.rotation_type == 1);
+    1240             :   }
+    1241             : 
+    1242             :   // | -------------------- desired throttle -------------------- |
+    1243             : 
+    1244       23978 :   double desired_thrust_force = f.dot(R.col(2));
+    1245       23978 :   double throttle             = 0;
+    1246             : 
+    1247       23978 :   if (tracker_command.use_throttle) {
+    1248             : 
+    1249             :     // the throttle is overriden from the tracker command
+    1250           0 :     throttle = tracker_command.throttle;
+    1251             : 
+    1252       23978 :   } else if (rampup_active_) {
+    1253             : 
+    1254             :     // deactivate the rampup when the times up
+    1255          15 :     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           1 :       double rampup_dt = (ros::Time::now() - rampup_last_time_).toSec();
+    1264             : 
+    1265           1 :       rampup_throttle_ += double(rampup_direction_) * _rampup_speed_ * rampup_dt;
+    1266             : 
+    1267           1 :       rampup_last_time_ = ros::Time::now();
+    1268             : 
+    1269           1 :       throttle = rampup_throttle_;
+    1270             : 
+    1271           1 :       ROS_INFO_THROTTLE(0.1, "[Se3Controller]: ramping up throttle, %.4f", throttle);
+    1272             :     }
+    1273             : 
+    1274             :   } else {
+    1275             : 
+    1276       23963 :     if (desired_thrust_force >= 0) {
+    1277       23963 :       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       23978 :   bool throttle_saturated = false;
+    1286             : 
+    1287       23978 :   if (!std::isfinite(throttle)) {
+    1288             : 
+    1289           0 :     ROS_ERROR("[Se3Controller]: NaN detected in variable 'throttle'!!!");
+    1290           0 :     return;
+    1291             : 
+    1292       23978 :   } 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       23978 :   } 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       23978 :   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       23978 :   Eigen::Vector3d unbiased_des_acc(0, 0, 0);
+    1321             : 
+    1322             :   {
+    1323       23978 :     Eigen::Vector3d unbiased_des_acc_world = (position_feedback + velocity_feedback) / total_mass + Ra;
+    1324             : 
+    1325       47956 :     geometry_msgs::Vector3Stamped world_accel;
+    1326             : 
+    1327       23978 :     world_accel.header.stamp    = ros::Time::now();
+    1328       23978 :     world_accel.header.frame_id = uav_state.header.frame_id;
+    1329       23978 :     world_accel.vector.x        = unbiased_des_acc_world(0);
+    1330       23978 :     world_accel.vector.y        = unbiased_des_acc_world(1);
+    1331       23978 :     world_accel.vector.z        = unbiased_des_acc_world(2);
+    1332             : 
+    1333       71934 :     auto res = common_handlers_->transformer->transformSingle(world_accel, "fcu");
+    1334             : 
+    1335       23978 :     if (res) {
+    1336       23978 :       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       23978 :   last_control_output_.desired_orientation = mrs_lib::AttitudeConverter(Rd);
+    1344             : 
+    1345             :   // fill the unbiased desired accelerations
+    1346       23978 :   last_control_output_.desired_unbiased_acceleration = unbiased_des_acc;
+    1347             : 
+    1348             :   // | ----------------- fill in the diagnostics ---------------- |
+    1349             : 
+    1350       23978 :   last_control_output_.diagnostics.ramping_up = rampup_active_;
+    1351             : 
+    1352       23978 :   last_control_output_.diagnostics.mass_estimator  = true;
+    1353       23978 :   last_control_output_.diagnostics.mass_difference = uav_mass_difference_;
+    1354       23978 :   last_control_output_.diagnostics.total_mass      = total_mass;
+    1355             : 
+    1356       23978 :   last_control_output_.diagnostics.disturbance_estimator = true;
+    1357             : 
+    1358       23978 :   last_control_output_.diagnostics.disturbance_bx_b = -Ib_b_(0);
+    1359       23978 :   last_control_output_.diagnostics.disturbance_by_b = -Ib_b_(1);
+    1360             : 
+    1361       23978 :   last_control_output_.diagnostics.disturbance_bx_w = -Ib_w(0);
+    1362       23978 :   last_control_output_.diagnostics.disturbance_by_w = -Ib_w(1);
+    1363             : 
+    1364       23978 :   last_control_output_.diagnostics.disturbance_wx_w = -Iw_w_(0);
+    1365       23978 :   last_control_output_.diagnostics.disturbance_wy_w = -Iw_w_(1);
+    1366             : 
+    1367       23978 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+    1368             : 
+    1369       23978 :   last_control_output_.diagnostics.controller = "Se3Controller";
+    1370             : 
+    1371             :   // | ------------ construct the attitude reference ------------ |
+    1372             : 
+    1373       23978 :   mrs_msgs::HwApiAttitudeCmd attitude_cmd;
+    1374             : 
+    1375       23978 :   attitude_cmd.stamp       = ros::Time::now();
+    1376       23978 :   attitude_cmd.orientation = mrs_lib::AttitudeConverter(Rd);
+    1377       23978 :   attitude_cmd.throttle    = throttle;
+    1378             : 
+    1379       23978 :   if (output_modality == common::ATTITUDE) {
+    1380             : 
+    1381        1199 :     last_control_output_.control_output = attitude_cmd;
+    1382             : 
+    1383        1199 :     return;
+    1384             :   }
+    1385             : 
+    1386             :   // --------------------------------------------------------------
+    1387             :   // |                      attitude control                      |
+    1388             :   // --------------------------------------------------------------
+    1389             : 
+    1390       22779 :   Eigen::Vector3d rate_feedforward = Eigen::Vector3d::Zero(3);
+    1391             : 
+    1392       22779 :   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       22779 :   } else if (tracker_command.use_heading_rate) {
+    1397             : 
+    1398             :     // to fill in the feed forward yaw rate
+    1399       22777 :     double desired_yaw_rate = 0;
+    1400             : 
+    1401             :     try {
+    1402       22777 :       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       22777 :     rate_feedforward << 0, 0, desired_yaw_rate;
+    1409             :   }
+    1410             : 
+    1411             :   // | ------------ jerk feedforward -> angular rate ------------ |
+    1412             : 
+    1413       22779 :   Eigen::Vector3d jerk_feedforward = Eigen::Vector3d(0, 0, 0);
+    1414             : 
+    1415       22779 :   if (tracker_command.use_jerk && drs_params.jerk_feedforward) {
+    1416             : 
+    1417       22758 :     ROS_DEBUG_THROTTLE(1.0, "[Se3Controller]: using jerk feedforward");
+    1418             : 
+    1419       22758 :     Eigen::Matrix3d I;
+    1420       22758 :     I << 0, 1, 0, -1, 0, 0, 0, 0, 0;
+    1421       22758 :     Eigen::Vector3d desired_jerk = Eigen::Vector3d(tracker_command.jerk.x, tracker_command.jerk.y, tracker_command.jerk.z);
+    1422       22758 :     jerk_feedforward             = (I.transpose() * Rd.transpose() * desired_jerk) / (desired_thrust_force / total_mass);
+    1423             :   }
+    1424             : 
+    1425             :   // | --------------- run the attitude controller -------------- |
+    1426             : 
+    1427       22779 :   Eigen::Vector3d attitude_rate_saturation(constraints.roll_rate, constraints.pitch_rate, constraints.yaw_rate);
+    1428             : 
+    1429       22779 :   auto attitude_rate_command = common::attitudeController(uav_state, attitude_cmd, jerk_feedforward + rate_feedforward, attitude_rate_saturation, Kq,
+    1430       45558 :                                                           drs_params.pitch_roll_heading_rate_compensation);
+    1431             : 
+    1432       22779 :   if (!attitude_rate_command) {
+    1433           0 :     return;
+    1434             :   }
+    1435             : 
+    1436             :   // | --------- fill in the already known attitude rate -------- |
+    1437             : 
+    1438             :   {
+    1439             :     try {
+    1440       22779 :       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       22779 :   if (output_modality == common::ATTITUDE_RATE) {
+    1449             : 
+    1450       20170 :     last_control_output_.control_output = attitude_rate_command;
+    1451             : 
+    1452       20170 :     return;
+    1453             :   }
+    1454             : 
+    1455             :   // --------------------------------------------------------------
+    1456             :   // |                    Attitude rate control                   |
+    1457             :   // --------------------------------------------------------------
+    1458             : 
+    1459        2609 :   Kw = common_handlers_->detailed_model_params->inertia.diagonal().array() * Kw;
+    1460             : 
+    1461        2609 :   auto control_group_command = common::attitudeRateController(uav_state, attitude_rate_command.value(), Kw);
+    1462             : 
+    1463        2609 :   if (!control_group_command) {
+    1464           0 :     return;
+    1465             :   }
+    1466             : 
+    1467        2609 :   if (output_modality == common::CONTROL_GROUP) {
+    1468             : 
+    1469        1307 :     last_control_output_.control_output = control_group_command;
+    1470             : 
+    1471        1307 :     return;
+    1472             :   }
+    1473             : 
+    1474             :   // --------------------------------------------------------------
+    1475             :   // |                        output mixer                        |
+    1476             :   // --------------------------------------------------------------
+    1477             : 
+    1478        2604 :   mrs_msgs::HwApiActuatorCmd actuator_cmd = common::actuatorMixer(control_group_command.value(), common_handlers_->detailed_model_params->control_group_mixer);
+    1479             : 
+    1480        1302 :   last_control_output_.control_output = actuator_cmd;
+    1481             : 
+    1482        1302 :   return;
+    1483             : }
+    1484             : 
+    1485             : //}
+    1486             : 
+    1487             : /* positionPassthrough() //{ */
+    1488             : 
+    1489         225 : void Se3Controller::positionPassthrough(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command) {
+    1490             : 
+    1491         225 :   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         450 :   mrs_msgs::HwApiPositionCmd cmd;
+    1497             : 
+    1498         225 :   cmd.header.frame_id = uav_state.header.frame_id;
+    1499         225 :   cmd.header.stamp    = ros::Time::now();
+    1500             : 
+    1501         225 :   cmd.position = tracker_command.position;
+    1502         225 :   cmd.heading  = tracker_command.heading;
+    1503             : 
+    1504         225 :   last_control_output_.control_output = cmd;
+    1505             : 
+    1506             :   // fill the unbiased desired accelerations
+    1507         225 :   last_control_output_.desired_unbiased_acceleration = {};
+    1508         225 :   last_control_output_.desired_orientation           = {};
+    1509         225 :   last_control_output_.desired_heading_rate          = {};
+    1510             : 
+    1511             :   // | ----------------- fill in the diagnostics ---------------- |
+    1512             : 
+    1513         225 :   last_control_output_.diagnostics.ramping_up = false;
+    1514             : 
+    1515         225 :   last_control_output_.diagnostics.mass_estimator  = false;
+    1516         225 :   last_control_output_.diagnostics.mass_difference = 0;
+    1517             : 
+    1518         225 :   last_control_output_.diagnostics.disturbance_estimator = false;
+    1519             : 
+    1520         225 :   last_control_output_.diagnostics.disturbance_bx_b = 0;
+    1521         225 :   last_control_output_.diagnostics.disturbance_by_b = 0;
+    1522             : 
+    1523         225 :   last_control_output_.diagnostics.disturbance_bx_w = 0;
+    1524         225 :   last_control_output_.diagnostics.disturbance_by_w = 0;
+    1525             : 
+    1526         225 :   last_control_output_.diagnostics.disturbance_wx_w = 0;
+    1527         225 :   last_control_output_.diagnostics.disturbance_wy_w = 0;
+    1528             : 
+    1529         225 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+    1530             : 
+    1531         225 :   last_control_output_.diagnostics.controller = "Se3Controller";
+    1532             : }
+    1533             : 
+    1534             : //}
+    1535             : 
+    1536             : /* PIDVelocityOutput() //{ */
+    1537             : 
+    1538        3109 : 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        3109 :   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        3109 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    1547        3109 :   auto gains       = mrs_lib::get_mutexed(mutex_gains_, gains_);
+    1548             : 
+    1549        3109 :   Eigen::Vector3d pos_ref = Eigen::Vector3d(tracker_command.position.x, tracker_command.position.y, tracker_command.position.z);
+    1550        3109 :   Eigen::Vector3d pos     = Eigen::Vector3d(uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z);
+    1551             : 
+    1552        3109 :   double hdg_ref = tracker_command.heading;
+    1553        3109 :   double hdg     = getHeadingSafely(uav_state, tracker_command);
+    1554             : 
+    1555             :   // | ------------------ velocity feedforward ------------------ |
+    1556             : 
+    1557        3109 :   Eigen::Vector3d vel_ff(0, 0, 0);
+    1558             : 
+    1559        3109 :   if (tracker_command.use_velocity_horizontal && tracker_command.use_velocity_vertical) {
+    1560        3109 :     vel_ff = Eigen::Vector3d(tracker_command.velocity.x, tracker_command.velocity.y, tracker_command.velocity.z);
+    1561             :   }
+    1562             : 
+    1563             :   // | -------------------------- gains ------------------------- |
+    1564             : 
+    1565        3109 :   Eigen::Vector3d Kp;
+    1566             : 
+    1567             :   {
+    1568        3109 :     std::scoped_lock lock(mutex_gains_);
+    1569             : 
+    1570        3109 :     Kp << gains.kpxy, gains.kpxy, gains.kpz;
+    1571             :   }
+    1572             : 
+    1573             :   // | --------------------- control errors --------------------- |
+    1574             : 
+    1575        3109 :   Eigen::Vector3d Ep = pos_ref - pos;
+    1576             : 
+    1577             :   // | --------------------------- pid -------------------------- |
+    1578             : 
+    1579        3109 :   position_pid_x_.setSaturation(constraints.horizontal_speed);
+    1580        3109 :   position_pid_y_.setSaturation(constraints.horizontal_speed);
+    1581        3109 :   position_pid_z_.setSaturation(std::min(constraints.vertical_ascending_speed, constraints.vertical_descending_speed));
+    1582             : 
+    1583        3109 :   double des_vel_x = position_pid_x_.update(Ep(0), dt);
+    1584        3109 :   double des_vel_y = position_pid_y_.update(Ep(1), dt);
+    1585        3109 :   double des_vel_z = position_pid_z_.update(Ep(2), dt);
+    1586             : 
+    1587             :   // | -------------------- position feedback ------------------- |
+    1588             : 
+    1589        3109 :   Eigen::Vector3d des_vel = Eigen::Vector3d(des_vel_x, des_vel_y, des_vel_z) + vel_ff;
+    1590             : 
+    1591        3109 :   if (control_output == common::VELOCITY_HDG) {
+    1592             : 
+    1593             :     // | --------------------- fill the output -------------------- |
+    1594             : 
+    1595        2336 :     mrs_msgs::HwApiVelocityHdgCmd cmd;
+    1596             : 
+    1597        1168 :     cmd.header.frame_id = uav_state.header.frame_id;
+    1598        1168 :     cmd.header.stamp    = ros::Time::now();
+    1599             : 
+    1600        1168 :     cmd.velocity.x = des_vel(0);
+    1601        1168 :     cmd.velocity.y = des_vel(1);
+    1602        1168 :     cmd.velocity.z = des_vel(2);
+    1603             : 
+    1604        1168 :     cmd.heading = tracker_command.heading;
+    1605             : 
+    1606        1168 :     last_control_output_.control_output = cmd;
+    1607             : 
+    1608        1941 :   } else if (control_output == common::VELOCITY_HDG_RATE) {
+    1609             : 
+    1610        1941 :     position_pid_heading_.setSaturation(constraints.heading_speed);
+    1611             : 
+    1612        1941 :     double hdg_err = mrs_lib::geometry::sradians::diff(hdg_ref, hdg);
+    1613             : 
+    1614        1941 :     double des_hdg_rate = position_pid_heading_.update(hdg_err, dt);
+    1615             : 
+    1616             :     // | --------------------------- ff --------------------------- |
+    1617             : 
+    1618        1941 :     double des_hdg_ff = 0;
+    1619             : 
+    1620        1941 :     if (tracker_command.use_heading_rate) {
+    1621        1941 :       des_hdg_ff = tracker_command.heading_rate;
+    1622             :     }
+    1623             : 
+    1624             :     // | --------------------- fill the output -------------------- |
+    1625             : 
+    1626        3882 :     mrs_msgs::HwApiVelocityHdgRateCmd cmd;
+    1627             : 
+    1628        1941 :     cmd.header.frame_id = uav_state.header.frame_id;
+    1629        1941 :     cmd.header.stamp    = ros::Time::now();
+    1630             : 
+    1631        1941 :     cmd.velocity.x = des_vel(0);
+    1632        1941 :     cmd.velocity.y = des_vel(1);
+    1633        1941 :     cmd.velocity.z = des_vel(2);
+    1634             : 
+    1635        1941 :     cmd.heading_rate = des_hdg_rate + des_hdg_ff;
+    1636             : 
+    1637        1941 :     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        3109 :   last_control_output_.desired_unbiased_acceleration = {};
+    1646        3109 :   last_control_output_.desired_orientation           = {};
+    1647        3109 :   last_control_output_.desired_heading_rate          = {};
+    1648             : 
+    1649             :   // | ----------------- fill in the diagnostics ---------------- |
+    1650             : 
+    1651        3109 :   last_control_output_.diagnostics.ramping_up = false;
+    1652             : 
+    1653        3109 :   last_control_output_.diagnostics.mass_estimator  = false;
+    1654        3109 :   last_control_output_.diagnostics.mass_difference = 0;
+    1655             : 
+    1656        3109 :   last_control_output_.diagnostics.disturbance_estimator = false;
+    1657             : 
+    1658        3109 :   last_control_output_.diagnostics.disturbance_bx_b = 0;
+    1659        3109 :   last_control_output_.diagnostics.disturbance_by_b = 0;
+    1660             : 
+    1661        3109 :   last_control_output_.diagnostics.disturbance_bx_w = 0;
+    1662        3109 :   last_control_output_.diagnostics.disturbance_by_w = 0;
+    1663             : 
+    1664        3109 :   last_control_output_.diagnostics.disturbance_wx_w = 0;
+    1665        3109 :   last_control_output_.diagnostics.disturbance_wy_w = 0;
+    1666             : 
+    1667        3109 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+    1668             : 
+    1669        3109 :   last_control_output_.diagnostics.controller = "Se3Controller";
+    1670             : }
+    1671             : 
+    1672             : //}
+    1673             : 
+    1674             : // --------------------------------------------------------------
+    1675             : // |                          callbacks                         |
+    1676             : 
+    1677             : 
+    1678             : /* //{ callbackDrs() */
+    1679             : 
+    1680         219 : void Se3Controller::callbackDrs(mrs_uav_controllers::se3_controllerConfig& config, [[maybe_unused]] uint32_t level) {
+    1681             : 
+    1682         219 :   mrs_lib::set_mutexed(mutex_drs_params_, config, drs_params_);
+    1683             : 
+    1684         219 :   ROS_INFO("[Se3Controller]: DRS updated gains");
+    1685         219 : }
+    1686             : 
+    1687             : //}
+    1688             : 
+    1689             : // --------------------------------------------------------------
+    1690             : // |                           timers                           |
+    1691             : // --------------------------------------------------------------
+    1692             : 
+    1693             : /* timerGains() //{ */
+    1694             : 
+    1695        4711 : void Se3Controller::timerGains(const ros::TimerEvent& event) {
+    1696             : 
+    1697        9422 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerGains", _gain_filtering_rate_, 1.0, event);
+    1698        9422 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("Se3Controller::timerGains", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1699             : 
+    1700        4711 :   auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+    1701        4711 :   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        4711 :   bool   bypass_filter = (mute_gains_ || mute_gains_by_tracker_);
+    1706        4711 :   double gain_coeff    = (mute_gains_ || mute_gains_by_tracker_) ? _gain_mute_coefficient_ : 1.0;
+    1707             : 
+    1708        4711 :   mute_gains_ = false;
+    1709             : 
+    1710        4711 :   double dt = (event.current_real - event.last_real).toSec();
+    1711             : 
+    1712        4711 :   if (!std::isfinite(dt) || (dt <= 0) || (dt > 5 * (1.0 / _gain_filtering_rate_))) {
+    1713          27 :     return;
+    1714             :   }
+    1715             : 
+    1716        4684 :   bool updated = false;
+    1717             : 
+    1718        4684 :   gains.kpxy          = calculateGainChange(dt, gains.kpxy, drs_params.kpxy * gain_coeff, bypass_filter, "kpxy", updated);
+    1719        4684 :   gains.kvxy          = calculateGainChange(dt, gains.kvxy, drs_params.kvxy * gain_coeff, bypass_filter, "kvxy", updated);
+    1720        4684 :   gains.kaxy          = calculateGainChange(dt, gains.kaxy, drs_params.kaxy * gain_coeff, bypass_filter, "kaxy", updated);
+    1721        4684 :   gains.kiwxy         = calculateGainChange(dt, gains.kiwxy, drs_params.kiwxy * gain_coeff, bypass_filter, "kiwxy", updated);
+    1722        4684 :   gains.kibxy         = calculateGainChange(dt, gains.kibxy, drs_params.kibxy * gain_coeff, bypass_filter, "kibxy", updated);
+    1723        4684 :   gains.kpz           = calculateGainChange(dt, gains.kpz, drs_params.kpz * gain_coeff, bypass_filter, "kpz", updated);
+    1724        4684 :   gains.kvz           = calculateGainChange(dt, gains.kvz, drs_params.kvz * gain_coeff, bypass_filter, "kvz", updated);
+    1725        4684 :   gains.kaz           = calculateGainChange(dt, gains.kaz, drs_params.kaz * gain_coeff, bypass_filter, "kaz", updated);
+    1726        4684 :   gains.kq_roll_pitch = calculateGainChange(dt, gains.kq_roll_pitch, drs_params.kq_roll_pitch * gain_coeff, bypass_filter, "kq_roll_pitch", updated);
+    1727        4684 :   gains.kq_yaw        = calculateGainChange(dt, gains.kq_yaw, drs_params.kq_yaw * gain_coeff, bypass_filter, "kq_yaw", updated);
+    1728        4684 :   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        4684 :   gains.kiwxy_lim = calculateGainChange(dt, gains.kiwxy_lim, drs_params.kiwxy_lim, false, "kiwxy_lim", updated);
+    1732        4684 :   gains.kibxy_lim = calculateGainChange(dt, gains.kibxy_lim, drs_params.kibxy_lim, false, "kibxy_lim", updated);
+    1733        4684 :   gains.km_lim    = calculateGainChange(dt, gains.km_lim, drs_params.km_lim, false, "km_lim", updated);
+    1734             : 
+    1735        4684 :   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        4684 :   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       65576 : 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       65576 :   double change = desired_value - current_value;
+    1774             : 
+    1775       65576 :   double gains_filter_max_change = _gains_filter_change_rate_ * dt;
+    1776       65576 :   double gains_filter_min_change = _gains_filter_min_change_rate_ * dt;
+    1777             : 
+    1778       65576 :   if (!bypass_rate) {
+    1779             : 
+    1780             :     // if current value is near 0...
+    1781             :     double change_in_perc;
+    1782             :     double saturated_change;
+    1783             : 
+    1784       65576 :     if (std::abs(current_value) < 1e-6) {
+    1785           0 :       change *= gains_filter_max_change;
+    1786             :     } else {
+    1787             : 
+    1788       65576 :       saturated_change = change;
+    1789             : 
+    1790       65576 :       change_in_perc = ((current_value + saturated_change) / current_value) - 1.0;
+    1791             : 
+    1792       65576 :       if (change_in_perc > gains_filter_max_change) {
+    1793        1269 :         saturated_change = current_value * gains_filter_max_change;
+    1794       64307 :       } else if (change_in_perc < -gains_filter_max_change) {
+    1795         162 :         saturated_change = current_value * -gains_filter_max_change;
+    1796             :       }
+    1797             : 
+    1798       65576 :       if (std::abs(saturated_change) < std::abs(change) * gains_filter_min_change) {
+    1799          36 :         change *= gains_filter_min_change;
+    1800             :       } else {
+    1801       65540 :         change = saturated_change;
+    1802             :       }
+    1803             :     }
+    1804             :   }
+    1805             : 
+    1806       65576 :   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       65576 :   return current_value + change;
+    1812             : }
+    1813             : 
+    1814             : //}
+    1815             : 
+    1816             : /* getHeadingSafely() //{ */
+    1817             : 
+    1818       28231 : double Se3Controller::getHeadingSafely(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command) {
+    1819             : 
+    1820             :   try {
+    1821       28231 :     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         109 : 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..eab13a373d --- /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-12-17 22:35:29Functions: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..b639e13ca6 --- /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-12-17 22:35:29Functions: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..bec5ed4388 --- /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-12-17 22:35:29Functions: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..686bb878a0 --- /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-12-17 22:35:29Functions: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..4b6b9fa797 --- /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-12-17 22:35:29Functions: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..5aaf6f7725 --- /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-12-17 22:35:29Functions: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..5177f1e022 --- /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-12-17 22:35:29Functions: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&)508
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1086
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1137
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1439
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2474
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3976
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)4022
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)8249
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)103535
mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::PublisherVisitor(mrs_uav_managers::control_manager::OutputPublisher*)126426
+
+
+ + + +
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..b04273e455 --- /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-12-17 22:35:29Functions: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*)126426
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)4022
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)8249
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)508
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1439
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)103535
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3976
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1086
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2474
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1137
+
+
+ + + +
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..2acfe85e8e --- /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-12-17 22:35:29Functions: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      126426 :     PublisherVisitor(OutputPublisher* obj) : obj_(obj){};
+      48             : 
+      49             :     OutputPublisher* obj_;
+      50             : 
+      51             :     template <class T>
+      52      126426 :     void operator()(const T& msg) {
+      53      126426 :       obj_->publish(msg);
+      54      126426 :     }
+      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-12-17 22:35:29Functions: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&)80
mrs_uav_managers::AglEstimator::~AglEstimator().280
+
+
+ + + +
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..176bde859d --- /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-12-17 22:35:29Functions: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&)80
mrs_uav_managers::AglEstimator::~AglEstimator()0
mrs_uav_managers::AglEstimator::~AglEstimator().280
+
+
+ + + +
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..fc25cdb3c5 --- /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-12-17 22:35:29Functions: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          80 :   AglEstimator(const std::string &name, const std::string &frame_id, const std::string &package_name)
+      55          80 :       : Estimator(agl::type, name, frame_id), package_name_(package_name) {
+      56          80 :   }
+      57             : 
+      58          80 :   virtual ~AglEstimator(void) {
+      59          80 :   }
+      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..04cfd7e6af --- /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:539655.2 %
Date:2024-12-17 22:35:29Functions:192770.4 %
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::HwApiVelocityHdgRateCmd_<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::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&)508
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)508
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&)1086
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1086
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&)1137
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1137
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&)1439
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1439
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&)2474
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2589
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&)3976
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&)4022
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)5368
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)5416
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&)8249
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)13785
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)15083
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&)88456
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)106244
+
+
+ + + +
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..0022bfa4ba --- /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:539655.2 %
Date:2024-12-17 22:35:29Functions:192770.4 %
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&)4022
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&)8249
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&)508
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&)1439
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&)88456
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&)3976
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&)1086
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&)2474
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&)1137
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&)15083
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&)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::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)5416
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)13785
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)508
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1439
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)106244
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)5368
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1086
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2589
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1137
+
+
+ + + +
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..138994477b --- /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:539655.2 %
Date:2024-12-17 22:35:29Functions:192770.4 %
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        5416 :   std::optional<double> operator()(const mrs_msgs::HwApiActuatorCmd& msg) {
+      82             : 
+      83        5416 :     if (msg.motors.size() == 0) {
+      84           0 :       return std::nullopt;
+      85             :     }
+      86             : 
+      87        5416 :     double throttle = 0;
+      88             : 
+      89       27080 :     for (size_t i = 0; i < msg.motors.size(); i++) {
+      90       21664 :       throttle += msg.motors.at(i);
+      91             :     };
+      92             : 
+      93        5416 :     throttle /= msg.motors.size();
+      94             : 
+      95        5416 :     return throttle;
+      96             :   }
+      97        5368 :   std::optional<double> operator()(const mrs_msgs::HwApiControlGroupCmd& msg) {
+      98        5368 :     return msg.throttle;
+      99             :   }
+     100       13785 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeCmd& msg) {
+     101       13785 :     return msg.throttle;
+     102             :   }
+     103      106244 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeRateCmd& msg) {
+     104      106244 :     return msg.throttle;
+     105             :   }
+     106        1137 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiAccelerationHdgRateCmd& msg) {
+     107        1137 :     return std::nullopt;
+     108             :   }
+     109        1086 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiAccelerationHdgCmd& msg) {
+     110        1086 :     return std::nullopt;
+     111             :   }
+     112        2589 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgRateCmd& msg) {
+     113        2589 :     return std::nullopt;
+     114             :   }
+     115        1439 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgCmd& msg) {
+     116        1439 :     return std::nullopt;
+     117             :   }
+     118         508 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiPositionCmd& msg) {
+     119         508 :     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        4022 :   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        4022 :     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        4022 :     return validateHwApiActuatorCmd(msg, node_name, var_name);
+     152             :   }
+     153        3976 :   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        3976 :     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        3976 :     return validateHwApiControlGroupCmd(msg, node_name, var_name);
+     162             :   }
+     163        8249 :   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        8249 :     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        8249 :     return validateHwApiAttitudeCmd(msg, node_name, var_name);
+     172             :   }
+     173       88456 :   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       88456 :     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       88456 :     return validateHwApiAttitudeRateCmd(msg, node_name, var_name);
+     182             :   }
+     183        1137 :   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        1137 :     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        1137 :     return validateHwApiAccelerationHdgRateCmd(msg, node_name, var_name);
+     192             :   }
+     193        1086 :   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        1086 :     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        1086 :     return validateHwApiAccelerationHdgCmd(msg, node_name, var_name);
+     202             :   }
+     203        2474 :   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        2474 :     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        2474 :     return validateHwApiVelocityHdgRateCmd(msg, node_name, var_name);
+     212             :   }
+     213        1439 :   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        1439 :     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        1439 :     return validateHwApiVelocityHdgCmd(msg, node_name, var_name);
+     222             :   }
+     223         508 :   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         508 :     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         508 :     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       15083 :   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       15083 :     initializeHwApiCmd(msg, min_throttle);
+     267       15083 :   }
+     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           0 :   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           0 :     initializeHwApiCmd(msg, uav_state);
+     279           0 :   }
+     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..ae8715691990bae086ba95dad2bda32a5157e3ef GIT binary patch literal 936 zcmV;Z16TZsP)8$oMOfwoEbkk%?&H<7fC8styUCF6PPJe1% z_gmVaWp93Y9Y)Q18fo;)0WYrzox(pZh`^9(mym_9VR1Jiyj^GC-yRcl(E zrJ`n-U9PS(ceN52Gh?3!fGh&+Dhr}U1`QK}q8hFPw$%Z)3`b!z^Gvx}YnmHvtVc!1 zh;Ey*F%=0v$?i{!m3co93ly=;^E3?;QQknOU|z)MVH~)w>$YvLP05;;H&AA710Y$Z z$%Hx2%Rw;osAr|`9kKxNFCx~cM%9jXVthUtX{O7f29{I$i=sB^as*6eK?FR^N1c(t zbl!MR({?IcZ~D@x_cM6mG5wfsFnvNZ-2F$FT3cDqCLmm{;sy!K9)t$R60vXMiQ$o=6n{0hS!s z0a&2AC!%>x7T6XVn`rGYZK0Bv?hd6omZn*h=923l54yuM`$r(cjcP@GOfnO2a|tsz zH#BBm_hSa*rUNtIm@|>G0%o2$bJsa@LuO8zGrMZja9+d=#tw~{?T*aYQPmXnV@9|i z0+%rpSF~SdCgA0#nThKel^OYOm;t<8#tha2{4O&QFHdiF4`$kWv)_B))8ma#{zZX1 z&FL_2e@egfN`8Ul_5t-~Q|4185843;w;&Hpk0aO}&I92lat|PYl>}3g6mZWs4J0Su zYPZdxNOx80vNE{R1C{{wH!LZ%in1n5$HoWyh%0^yp-?Zcn#De1+kve(E)2uGvC9<$66nK z4|Q93FCWqA`ZXVO-zRuL`l`^qo#Bj|b?RuLsnpo#8}@ + + + + + + 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:539655.2 %
Date:2024-12-17 22:35:29Functions:192770.4 %
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 +
55.2%55.2%
+ 		55.2 %53 / 9670.4 %19 / 27
<unnamed>55.2 %53 / 9670.4 %19 / 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..358254f5e8 --- /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:539655.2 %
Date:2024-12-17 22:35:29Functions:192770.4 %
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 +
55.2%55.2%
+ 		55.2 %53 / 9670.4 %19 / 27
<unnamed>55.2 %53 / 9670.4 %19 / 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..df68254186 --- /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:539655.2 %
Date:2024-12-17 22:35:29Functions:192770.4 %
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 +
55.2%55.2%
+ 		55.2 %53 / 9670.4 %19 / 27
<unnamed>55.2 %53 / 9670.4 %19 / 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..626af0c3ca --- /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:539655.2 %
Date:2024-12-17 22:35:29Functions:192770.4 %
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 +
55.2%55.2%
+ 		55.2 %53 / 9670.4 %19 / 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..282145ccb4 --- /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:539655.2 %
Date:2024-12-17 22:35:29Functions:192770.4 %
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 +
55.2%55.2%
+ 		55.2 %53 / 9670.4 %19 / 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..244b274b39 --- /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:539655.2 %
Date:2024-12-17 22:35:29Functions:192770.4 %
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 +
55.2%55.2%
+ 		55.2 %53 / 9670.4 %19 / 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..ca9676c86d --- /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-12-17 22:35:29Functions: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().2545
+
+
+ + + +
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..77bdf016f7 --- /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-12-17 22:35:29Functions: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().2545
+
+
+ + + +
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..9337ed1db6 --- /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-12-17 22:35:29Functions: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         545 :   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-12-17 22:35:29Functions: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&)625
mrs_uav_managers::Estimator::~Estimator().2625
+
+
+ + + +
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..208c4b1cc0 --- /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-12-17 22:35:29Functions: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&)625
mrs_uav_managers::Estimator::~Estimator()0
mrs_uav_managers::Estimator::~Estimator().2625
+
+
+ + + +
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..840c26684f --- /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-12-17 22:35:29Functions: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         625 :   Estimator(const std::string &type, const std::string &name, const std::string &frame_id) : type_(type), name_(name), frame_id_(frame_id) {
+      64         625 :   }
+      65             : 
+      66         625 :   virtual ~Estimator(void) {
+      67         625 :   }
+      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-12-17 22:35:29Functions: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..b2d0ddd504 --- /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-12-17 22:35:29Functions: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..74a142a0d8 --- /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-12-17 22:35:29Functions: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..d360e94741 --- /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-12-17 22:35:29Functions: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..d77f483a07 --- /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-12-17 22:35:29Functions: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..83f9f93ee3 --- /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-12-17 22:35:29Functions: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..4795fdccb1 --- /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-12-17 22:35:29Functions: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&)347
mrs_uav_managers::estimation_manager::Support::toLowercase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)436
mrs_uav_managers::estimation_manager::Support::toSnakeCase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2445
mrs_uav_managers::estimation_manager::Support::applyPoseDiff(geometry_msgs::Pose_<std::allocator<void> > const&, geometry_msgs::Pose_<std::allocator<void> > const&)176199
mrs_uav_managers::estimation_manager::Support::rotateVecByHdg(geometry_msgs::Vector3_<std::allocator<void> > const&, double)322428
mrs_uav_managers::estimation_manager::Support::msgFromTf2(tf2::Transform const&)371574
mrs_uav_managers::estimation_manager::Support::uavStateToOdom(mrs_msgs::UavState_<std::allocator<void> > const&)386511
mrs_uav_managers::estimation_manager::Support::rotateVector(geometry_msgs::Vector3_<std::allocator<void> > const&, geometry_msgs::Quaternion_<std::allocator<void> > const&)387803
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::Quaternion_<std::allocator<void> > const&)413663
mrs_uav_managers::estimation_manager::Support::pointToVector3(geometry_msgs::Point_<std::allocator<void> > const&)782182
mrs_uav_managers::estimation_manager::Support::poseFromTf2(tf2::Transform const&)792672
mrs_uav_managers::estimation_manager::Support::tf2FromPose(geometry_msgs::Pose_<std::allocator<void> > const&)1165055
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::TransformStamped_<std::allocator<void> > const&)1256491
+
+
+ + + +
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..6eabf280e8 --- /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-12-17 22:35:29Functions: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&)371574
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&)792672
mrs_uav_managers::estimation_manager::Support::tf2FromPose(geometry_msgs::Pose_<std::allocator<void> > const&)1165055
mrs_uav_managers::estimation_manager::Support::toLowercase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)436
mrs_uav_managers::estimation_manager::Support::toSnakeCase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2445
mrs_uav_managers::estimation_manager::Support::rotateVector(geometry_msgs::Vector3_<std::allocator<void> > const&, geometry_msgs::Quaternion_<std::allocator<void> > const&)387803
mrs_uav_managers::estimation_manager::Support::applyPoseDiff(geometry_msgs::Pose_<std::allocator<void> > const&, geometry_msgs::Pose_<std::allocator<void> > const&)176199
mrs_uav_managers::estimation_manager::Support::pointToVector3(geometry_msgs::Point_<std::allocator<void> > const&)782182
mrs_uav_managers::estimation_manager::Support::rotateVecByHdg(geometry_msgs::Vector3_<std::allocator<void> > const&, double)322428
mrs_uav_managers::estimation_manager::Support::uavStateToOdom(mrs_msgs::UavState_<std::allocator<void> > const&)386511
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&)347
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&)413663
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::TransformStamped_<std::allocator<void> > const&)1256491
+
+
+ + + +
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..7de07977d6 --- /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-12-17 22:35:29Functions: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        2445 :   static std::string toSnakeCase(const std::string& str_in) {
+      39             : 
+      40        2445 :     std::string str(1, tolower(str_in[0]));
+      41             : 
+      42       38335 :     for (auto it = str_in.begin() + 1; it != str_in.end(); ++it) {
+      43       35890 :       if (isupper(*it) && *(it - 1) != '_' && islower(*(it - 1))) {
+      44        2015 :         str += "_";
+      45             :       }
+      46       35890 :       str += *it;
+      47             :     }
+      48             : 
+      49        2445 :     std::transform(str.begin(), str.end(), str.begin(), ::tolower);
+      50             : 
+      51        2445 :     return str;
+      52             :   }
+      53             :   /*//}*/
+      54             : 
+      55             : /* toLowercase() //{ */
+      56             : 
+      57         436 : static std::string toLowercase(const std::string str_in) {
+      58         436 :   std::string str_out = str_in;
+      59         436 :   std::transform(str_out.begin(), str_out.end(), str_out.begin(), ::tolower);
+      60         436 :   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      322428 :   static tf2::Vector3 rotateVecByHdg(const geometry_msgs::Vector3& vec_in, const double hdg_in) {
+      99             : 
+     100      322428 :     const tf2::Quaternion q_hdg = mrs_lib::AttitudeConverter(0, 0, 0).setHeading(hdg_in);
+     101             : 
+     102      318757 :     const tf2::Vector3 vec_tf2(vec_in.x, vec_in.y, vec_in.z);
+     103             : 
+     104      318838 :     const tf2::Vector3 vec_rotated = tf2::quatRotate(q_hdg, vec_tf2);
+     105             : 
+     106      634426 :     return vec_rotated;
+     107             :   }
+     108             :   //}
+     109             : 
+     110             :   /* noNans() //{ */
+     111     1256491 :   static bool noNans(const geometry_msgs::TransformStamped& tf) {
+     112             : 
+     113     3768533 :     return (std::isfinite(tf.transform.rotation.x) && std::isfinite(tf.transform.rotation.y) && std::isfinite(tf.transform.rotation.z) &&
+     114     5024089 :             std::isfinite(tf.transform.rotation.w) && std::isfinite(tf.transform.translation.x) && std::isfinite(tf.transform.translation.y) &&
+     115     2512130 :             std::isfinite(tf.transform.translation.z));
+     116             :   }
+     117             :   //}
+     118             : 
+     119             :   /* noNans() //{ */
+     120      413663 :   static bool noNans(const geometry_msgs::Quaternion& q) {
+     121             : 
+     122      413663 :     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     1165055 :   static tf2::Transform tf2FromPose(const geometry_msgs::Pose& pose_in) {
+     136             : 
+     137     1165055 :     tf2::Vector3 position(pose_in.position.x, pose_in.position.y, pose_in.position.z);
+     138             : 
+     139     1164520 :     tf2::Quaternion q;
+     140     1164522 :     tf2::fromMsg(pose_in.orientation, q);
+     141             : 
+     142     1164391 :     tf2::Transform tf_out(q, position);
+     143             : 
+     144     2325820 :     return tf_out;
+     145             :   }
+     146             : 
+     147             :   //}
+     148             : 
+     149             :   /* poseFromTf2() //{ */
+     150             : 
+     151      792672 :   static geometry_msgs::Pose poseFromTf2(const tf2::Transform& tf_in) {
+     152             : 
+     153      792672 :     geometry_msgs::Pose pose_out;
+     154      790796 :     pose_out.position.x = tf_in.getOrigin().getX();
+     155      791275 :     pose_out.position.y = tf_in.getOrigin().getY();
+     156      791440 :     pose_out.position.z = tf_in.getOrigin().getZ();
+     157             : 
+     158      792821 :     pose_out.orientation = tf2::toMsg(tf_in.getRotation());
+     159             : 
+     160      792705 :     return pose_out;
+     161             :   }
+     162             : 
+     163             :   //}
+     164             : 
+     165             :   /* msgFromTf2() //{ */
+     166      371574 :   static geometry_msgs::Transform msgFromTf2(const tf2::Transform& tf_in) {
+     167             : 
+     168      371574 :     geometry_msgs::Transform tf_out;
+     169      371574 :     tf_out.translation.x = tf_in.getOrigin().getX();
+     170      371574 :     tf_out.translation.y = tf_in.getOrigin().getY();
+     171      371574 :     tf_out.translation.z = tf_in.getOrigin().getZ();
+     172      371574 :     tf_out.rotation = tf2::toMsg(tf_in.getRotation());
+     173             : 
+     174      371574 :     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      782182 :   static geometry_msgs::Vector3 pointToVector3(const geometry_msgs::Point& point_in) {
+     194             : 
+     195      782182 :     geometry_msgs::Vector3 vec_out;
+     196      781989 :     vec_out.x = point_in.x;
+     197      781989 :     vec_out.y = point_in.y;
+     198      781989 :     vec_out.z = point_in.z;
+     199             : 
+     200      781989 :     return vec_out;
+     201             :   }
+     202             : 
+     203             :   //}
+     204             : 
+     205             :   /*//{ rotateVector() */
+     206      387803 :   static geometry_msgs::Vector3 rotateVector(const geometry_msgs::Vector3& vec_in, const geometry_msgs::Quaternion& q_in) {
+     207             : 
+     208             :     try {
+     209      387803 :       Eigen::Matrix3d        R = mrs_lib::AttitudeConverter(q_in);
+     210      387784 :       Eigen::Vector3d        vec_in_eigen(vec_in.x, vec_in.y, vec_in.z);
+     211      387626 :       Eigen::Vector3d        vec_eigen_rotated = R * vec_in_eigen;
+     212      387665 :       geometry_msgs::Vector3 vec_out;
+     213      387424 :       vec_out.x = vec_eigen_rotated[0];
+     214      387252 :       vec_out.y = vec_eigen_rotated[1];
+     215      387473 :       vec_out.z = vec_eigen_rotated[2];
+     216      387541 :       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      386511 :   static nav_msgs::Odometry uavStateToOdom(const mrs_msgs::UavState& uav_state) {
+     227      386511 :     nav_msgs::Odometry odom;
+     228      386506 :     odom.header              = uav_state.header;
+     229      386487 :     odom.child_frame_id      = uav_state.child_frame_id;
+     230      386474 :     odom.pose.pose           = uav_state.pose;
+     231      386474 :     odom.twist.twist.angular = uav_state.velocity.angular;
+     232             : 
+     233      386474 :     tf2::Quaternion q;
+     234      386492 :     tf2::fromMsg(odom.pose.pose.orientation, q);
+     235      386479 :     odom.twist.twist.linear = Support::rotateVector(uav_state.velocity.linear, mrs_lib::AttitudeConverter(q.inverse()));
+     236             : 
+     237      772176 :     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      176199 :   static geometry_msgs::Pose applyPoseDiff(const geometry_msgs::Pose& pose_in, const geometry_msgs::Pose& pose_diff) {
+     265             : 
+     266      176199 :     tf2::Vector3    pos_in;
+     267      176199 :     tf2::Quaternion q_in;
+     268      176199 :     tf2::fromMsg(pose_in.position, pos_in);
+     269      176199 :     tf2::fromMsg(pose_in.orientation, q_in);
+     270             : 
+     271      176199 :     tf2::Vector3    pos_diff;
+     272      176199 :     tf2::Quaternion q_diff;
+     273      176199 :     tf2::fromMsg(pose_diff.position, pos_diff);
+     274      176199 :     tf2::fromMsg(pose_diff.orientation, q_diff);
+     275             : 
+     276      176199 :     const tf2::Vector3    pos_out = tf2::quatRotate(q_diff.inverse(), (pos_in - pos_diff));
+     277      176199 :     const tf2::Quaternion q_out   = q_diff.inverse() * q_in;
+     278             : 
+     279      176199 :     geometry_msgs::Pose pose_out;
+     280      176199 :     tf2::toMsg(pos_out, pose_out.position);
+     281      176199 :     pose_out.orientation = tf2::toMsg(q_out);
+     282             : 
+     283      352398 :     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         347 :   static bool isStringInVector(const std::string& value, const std::vector<std::string>& str_vec) {
+     300         347 :     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-12-17 22:35:29Functions: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
controller.h +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
<unnamed>100.0 %1 / 150.0 %1 / 2
tracker.h +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
<unnamed>100.0 %1 / 150.0 %1 / 2
agl_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
<unnamed>100.0 %5 / 566.7 %2 / 3
state_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..f8e42ea67e --- /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-12-17 22:35:29Functions: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
controller.h +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
<unnamed>100.0 %1 / 150.0 %1 / 2
tracker.h +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
<unnamed>100.0 %1 / 150.0 %1 / 2
agl_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
<unnamed>100.0 %5 / 566.7 %2 / 3
state_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..00425da5e4 --- /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-12-17 22:35:29Functions: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..0c22c20a6c --- /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-12-17 22:35:29Functions: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
controller.h +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
tracker.h +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
agl_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
state_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..b7c9fce7be --- /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-12-17 22:35:29Functions: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
controller.h +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
tracker.h +
100.0%
+ 		100.0 %1 / 150.0 %1 / 2
agl_estimator.h +
100.0%
+ 		100.0 %5 / 566.7 %2 / 3
state_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..54fd90bfda --- /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-12-17 22:35:29Functions: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..275810cf55 --- /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-12-17 22:35:29Functions: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&)117
mrs_uav_managers::StateEstimator::~StateEstimator().2117
+
+
+ + + +
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..ea850e2f3e --- /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-12-17 22:35:29Functions: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&)117
mrs_uav_managers::StateEstimator::~StateEstimator()0
mrs_uav_managers::StateEstimator::~StateEstimator().2117
+
+
+ + + +
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..a3a22fbfb2 --- /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-12-17 22:35:29Functions: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         117 :   StateEstimator(const std::string &name, const std::string &frame_id, const std::string &package_name)
+      62         117 :       : Estimator(state::type, name, frame_id), package_name_(package_name) {
+      63         117 :   }
+      64             : 
+      65         117 :   virtual ~StateEstimator(void) {
+      66         117 :   }
+      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..4e8c41b4d9 --- /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-12-17 22:35:29Functions: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().2655
+
+
+ + + +
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..b39e9c458c --- /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-12-17 22:35:29Functions: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().2655
+
+
+ + + +
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..06f97141f7 --- /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-12-17 22:35:29Functions: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         655 :   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:19540248.5 %
Date:2024-12-17 22:35:29Functions: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 +
79.9%79.9%
+ 		79.9 %195 / 24492.9 %13 / 14
<unnamed>79.9 %195 / 24492.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..5dc48f89b6 --- /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:19540248.5 %
Date:2024-12-17 22:35:29Functions: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 +
79.9%79.9%
+ 		79.9 %195 / 24492.9 %13 / 14
<unnamed>79.9 %195 / 24492.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..302a844970 --- /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:19540248.5 %
Date:2024-12-17 22:35:29Functions: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 +
79.9%79.9%
+ 		79.9 %195 / 24492.9 %13 / 14
<unnamed>79.9 %195 / 24492.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..c532bcece8 --- /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:19540248.5 %
Date:2024-12-17 22:35:29Functions: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 +
79.9%79.9%
+ 		79.9 %195 / 24492.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..7f73d4a73b --- /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:19540248.5 %
Date:2024-12-17 22:35:29Functions: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 +
79.9%79.9%
+ 		79.9 %195 / 24492.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..08b92b3a20 --- /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:19540248.5 %
Date:2024-12-17 22:35:29Functions: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 +
79.9%79.9%
+ 		79.9 %195 / 24492.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..4bf6feac0d --- /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-12-17 22:35:29Functions: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..ef786fca2b --- /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-12-17 22:35:29Functions: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..8db480c056 --- /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-12-17 22:35:29Functions: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:19524479.9 %
Date:2024-12-17 22:35:29Functions: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&)117
mrs_uav_managers::TfSource::setWorldOrigin(geometry_msgs::Point_<std::allocator<void> > const&)117
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)117
mrs_uav_managers::TfSource::getName[abi:cxx11]()1072
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> > >&)132323
mrs_uav_managers::TfSource::publishTfFromOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)211382
mrs_uav_managers::TfSource::callbackTfSourceOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)211457
mrs_uav_managers::TfSource::publishTfFromAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)218217
mrs_uav_managers::TfSource::callbackTfSourceAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)218228
mrs_uav_managers::TfSource::getPrintName[abi:cxx11]()988315
+
+
+ + + +
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..79d1fdfeaf --- /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:19524479.9 %
Date:2024-12-17 22:35:29Functions: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]()988315
mrs_uav_managers::TfSource::setUtmOrigin(geometry_msgs::Point_<std::allocator<void> > const&)117
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&)117
mrs_uav_managers::TfSource::publishTfFromAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)218217
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> > >&)132323
mrs_uav_managers::TfSource::publishTfFromOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)211382
mrs_uav_managers::TfSource::callbackTfSourceAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)218228
mrs_uav_managers::TfSource::callbackTfSourceOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)211457
mrs_uav_managers::TfSource::getName[abi:cxx11]()1072
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)117
+
+
+ + + +
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..061a6ad9ca --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.html @@ -0,0 +1,712 @@ + + + + + + + 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:19524479.9 %
Date:2024-12-17 22:35:29Functions: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         117 :   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         117 :       : name_(name), nh_(nh), broadcaster_(broadcaster), ch_(ch), is_utm_source_(is_utm_source) {
+      42             : 
+      43         117 :     ROS_INFO("[%s]: initializing", getPrintName().c_str());
+      44             : 
+      45         117 :     if (name != "dummy") {
+      46             : 
+      47             :       /*//{ load parameters */
+      48             : 
+      49         234 :       const std::string yaml_prefix = "mrs_uav_managers/transform_manager/";
+      50             : 
+      51         234 :       std::string odom_topic, attitude_topic, ns;
+      52             : 
+      53         117 :       param_loader->loadParam(yaml_prefix + getName() + "/odom_topic", odom_topic);
+      54         117 :       param_loader->loadParam(yaml_prefix + getName() + "/custom_frame_id/enabled", custom_frame_id_enabled_, false);
+      55         117 :       if (custom_frame_id_enabled_) {
+      56           0 :         param_loader->loadParam(yaml_prefix + getName() + "/custom_frame_id/frame_id", custom_frame_id_);
+      57             :       }
+      58         117 :       param_loader->loadParam(yaml_prefix + getName() + "/custom_child_frame_id/enabled", custom_child_frame_id_enabled_, false);
+      59         117 :       if (custom_child_frame_id_enabled_) {
+      60           0 :         param_loader->loadParam(yaml_prefix + getName() + "/custom_child_frame_id/frame_id", custom_child_frame_id_);
+      61             :       }
+      62         117 :       param_loader->loadParam(yaml_prefix + getName() + "/tf_from_attitude/enabled", tf_from_attitude_enabled_);
+      63         117 :       if (tf_from_attitude_enabled_) {
+      64         116 :         param_loader->loadParam(yaml_prefix + getName() + "/tf_from_attitude/attitude_topic", attitude_topic);
+      65             :       }
+      66         117 :       param_loader->loadParam(yaml_prefix + getName() + "/namespace", ns);
+      67         117 :       full_topic_odom_     = "/" + ch_->uav_name + "/" + ns + "/" + odom_topic;
+      68         117 :       full_topic_attitude_ = "/" + ch_->uav_name + "/" + ns + "/" + attitude_topic;
+      69         117 :       param_loader->loadParam(yaml_prefix + getName() + "/inverted", is_inverted_);
+      70         117 :       param_loader->loadParam(yaml_prefix + getName() + "/republish_in_frames", republish_in_frames_);
+      71             : 
+      72             :       /* coordinate frames origins //{ */
+      73         117 :       param_loader->loadParam(yaml_prefix + getName() + "/utm_based", is_utm_based_);
+      74             :       /* param_loader->loadParam(yaml_prefix + getName() + "/publish_local_tf", publish_local_tf_); */
+      75             : 
+      76             :       /*//{ utm source */
+      77         117 :       if (is_utm_based_) {
+      78         232 :         std::string utm_origin_parent_frame_id;
+      79         116 :         param_loader->loadParam(yaml_prefix + "utm_origin_tf/parent", utm_origin_parent_frame_id);
+      80         116 :         ns_utm_origin_parent_frame_id_ = ch_->uav_name + "/" + utm_origin_parent_frame_id;
+      81             : 
+      82         116 :         std::string utm_origin_child_frame_id;
+      83         116 :         param_loader->loadParam(yaml_prefix + "utm_origin_tf/child", utm_origin_child_frame_id);
+      84         116 :         ns_utm_origin_child_frame_id_ = ch_->uav_name + "/" + utm_origin_child_frame_id;
+      85             :       }
+      86             :       /*//}*/
+      87             : 
+      88             :       /*//{ world source */
+      89         117 :       if (is_utm_based_) {
+      90         232 :         std::string world_origin_parent_frame_id;
+      91         116 :         param_loader->loadParam(yaml_prefix + "world_origin_tf/parent", world_origin_parent_frame_id);
+      92         116 :         ns_world_origin_parent_frame_id_ = ch_->uav_name + "/" + world_origin_parent_frame_id;
+      93             : 
+      94         116 :         std::string world_origin_child_frame_id;
+      95         116 :         param_loader->loadParam(yaml_prefix + "world_origin_tf/child", world_origin_child_frame_id);
+      96         116 :         ns_world_origin_child_frame_id_ = ch_->uav_name + "/" + world_origin_child_frame_id;
+      97             :       }
+      98             :       /*//}*/
+      99             : 
+     100             :       //}
+     101             : 
+     102         117 :       if (!param_loader->loadedSuccessfully()) {
+     103           0 :         ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
+     104           0 :         ros::shutdown();
+     105             :       }
+     106             : 
+     107             :       /*//}*/
+     108             : 
+     109             :       /*//{ initialize subscribers */
+     110         234 :       mrs_lib::SubscribeHandlerOptions shopts;
+     111         117 :       shopts.nh                 = nh_;
+     112         117 :       shopts.node_name          = getPrintName();
+     113         117 :       shopts.no_message_timeout = mrs_lib::no_timeout;
+     114         117 :       shopts.threadsafe         = true;
+     115         117 :       shopts.autostart          = true;
+     116         117 :       shopts.queue_size         = 10;
+     117         117 :       shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     118             : 
+     119         117 :       sh_tf_source_odom_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, full_topic_odom_, &TfSource::callbackTfSourceOdom, this);
+     120         117 :       if (tf_from_attitude_enabled_) {
+     121         116 :         sh_tf_source_att_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, full_topic_attitude_, &TfSource::callbackTfSourceAtt, this);
+     122             :       }
+     123             : 
+     124         117 :       if (is_utm_source_) {
+     125         108 :         sh_altitude_amsl_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude>(shopts, "altitude_amsl_in");
+     126             :       }
+     127             : 
+     128             :     }
+     129             :     /*//}*/
+     130             : 
+     131         200 :     for (auto frame_id : republish_in_frames_) {
+     132          83 :       republishers_.push_back(
+     133         166 :           std::make_pair(frame_id, mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh_, full_topic_odom_ + "/" + frame_id.substr(0, frame_id.find("_origin")))));
+     134             :     }
+     135         117 :     is_initialized_ = true;
+     136         117 :     ROS_INFO("[%s]: initialized", getPrintName().c_str());
+     137         117 :   }
+     138             :   /*//}*/
+     139             : 
+     140             :   /*//{ getName() */
+     141        1072 :   std::string getName() {
+     142        1072 :     return name_;
+     143             :   }
+     144             :   /*//}*/
+     145             : 
+     146             :   /*//{ getPrintName() */
+     147      988315 :   std::string getPrintName() {
+     148     1979490 :     return ch_->nodelet_name + "/" + name_;
+     149             :   }
+     150             :   /*//}*/
+     151             : 
+     152             :   /*//{ getIsUtmBased() */
+     153          20 :   bool getIsUtmBased() {
+     154          20 :     return is_utm_based_;
+     155             :   }
+     156             :   /*//}*/
+     157             : 
+     158             :   /*//{ getIsUtmSource() */
+     159          18 :   bool getIsUtmSource() {
+     160          18 :     return is_utm_source_;
+     161             :   }
+     162             :   /*//}*/
+     163             : 
+     164             :   /*//{ setIsUtmSource() */
+     165          10 :   void setIsUtmSource(const bool is_utm_source) {
+     166          10 :     if (is_utm_source) {
+     167           5 :       mrs_lib::SubscribeHandlerOptions shopts;
+     168           5 :       shopts.nh                 = nh_;
+     169           5 :       shopts.node_name          = getPrintName();
+     170           5 :       shopts.no_message_timeout = mrs_lib::no_timeout;
+     171           5 :       shopts.threadsafe         = true;
+     172           5 :       shopts.autostart          = true;
+     173           5 :       shopts.queue_size         = 10;
+     174           5 :       shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     175           5 :       sh_altitude_amsl_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude>(shopts, "altitude_amsl_in");
+     176             :     }
+     177          10 :     is_utm_source_ = is_utm_source;
+     178          10 :   }
+     179             :   /*//}*/
+     180             : 
+     181             :   /*//{ setUtmOrigin() */
+     182         117 :   void setUtmOrigin(const geometry_msgs::Point& pt) {
+     183             : 
+     184         117 :     if (is_utm_based_ && !is_utm_origin_set_) {
+     185         116 :       utm_origin_        = pt;
+     186         116 :       is_utm_origin_set_ = true;
+     187             :     }
+     188         117 :   }
+     189             :   /*//}*/
+     190             : 
+     191             :   /*//{ setWorldOrigin() */
+     192         117 :   void setWorldOrigin(const geometry_msgs::Point& pt) {
+     193             : 
+     194         117 :     if (is_utm_based_ && !is_world_origin_set_) {
+     195         116 :       world_origin_        = pt;
+     196         116 :       is_world_origin_set_ = true;
+     197             :     }
+     198         117 :   }
+     199             :   /*//}*/
+     200             : 
+     201             : private:
+     202             :   const std::string name_;
+     203             :   const std::string ns_fcu_frame_id_;
+     204             : 
+     205             :   ros::NodeHandle nh_;
+     206             : 
+     207             :   std::shared_ptr<mrs_lib::TransformBroadcaster> broadcaster_;
+     208             :   tf2_ros::StaticTransformBroadcaster            static_broadcaster_;
+     209             : 
+     210             :   std::shared_ptr<estimation_manager::CommonHandlers_t> ch_;
+     211             : 
+     212             :   bool is_inverted_;
+     213             : 
+     214             :   bool             is_utm_based_;
+     215             :   bool             publish_local_tf_ = false;
+     216             :   bool             publish_utm_tf_   = false;
+     217             :   bool             publish_world_tf_ = false;
+     218             :   std::atomic_bool is_utm_source_    = false;
+     219             :   std::string      ns_utm_origin_parent_frame_id_;
+     220             :   std::string      ns_utm_origin_child_frame_id_;
+     221             : 
+     222             :   bool                 is_utm_origin_set_ = false;
+     223             :   geometry_msgs::Point utm_origin_;
+     224             : 
+     225             :   std::string ns_world_origin_parent_frame_id_;
+     226             :   std::string ns_world_origin_child_frame_id_;
+     227             : 
+     228             :   bool                 is_world_origin_set_ = false;
+     229             :   geometry_msgs::Point world_origin_;
+     230             : 
+     231             :   std::string full_topic_odom_;
+     232             :   std::string full_topic_attitude_;
+     233             :   bool        tf_from_attitude_enabled_ = false;
+     234             : 
+     235             :   bool        custom_frame_id_enabled_;
+     236             :   std::string custom_frame_id_;
+     237             : 
+     238             :   bool        custom_child_frame_id_enabled_;
+     239             :   std::string custom_child_frame_id_;
+     240             : 
+     241             :   std::atomic_bool is_initialized_               = false;
+     242             :   std::atomic_bool is_local_static_tf_published_ = false;
+     243             :   std::atomic_bool is_utm_static_tf_published_   = false;
+     244             :   std::atomic_bool is_world_static_tf_published_ = false;
+     245             : 
+     246             :   std::vector<std::string> republish_in_frames_;
+     247             : 
+     248             :   std::vector<std::pair<std::string, mrs_lib::PublisherHandler<nav_msgs::Odometry>>> republishers_;
+     249             : 
+     250             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry>               sh_tf_source_odom_;
+     251             :   mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_tf_source_att_;
+     252             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude>          sh_altitude_amsl_;
+     253             :   nav_msgs::OdometryConstPtr                                  first_msg_;
+     254             :   bool                                                        got_first_msg_ = false;
+     255             : 
+     256             : 
+     257             :   /*//{ callbackTfSourceOdom()*/
+     258      211457 :   void callbackTfSourceOdom(const nav_msgs::Odometry::ConstPtr msg) {
+     259             : 
+     260      211457 :     if (!is_initialized_) {
+     261           0 :       return;
+     262             :     }
+     263             : 
+     264      634302 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::callbackTfSourceOdom", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     265             : 
+     266      211402 :     scope_timer.checkpoint("get msg");
+     267             : 
+     268      211450 :     if (!got_first_msg_) {
+     269         117 :       first_msg_     = msg;
+     270         117 :       got_first_msg_ = true;
+     271             :     }
+     272             : 
+     273      211450 :     publishTfFromOdom(msg);
+     274      211466 :     scope_timer.checkpoint("pub tf");
+     275             : 
+     276      211452 :     if (publish_local_tf_ && !is_local_static_tf_published_) {
+     277           0 :       publishLocalTf(msg->header.frame_id);
+     278           0 :       scope_timer.checkpoint("pub local tf");
+     279             :     }
+     280             : 
+     281             :     /* if (publish_utm_tf_ && is_utm_based_ && is_utm_origin_set_ && !is_utm_static_tf_published_) { */
+     282             :     /*   publishUtmTf(msg->header.frame_id); */
+     283             :     /*   scope_timer.checkpoint("pub utm tf"); */
+     284             :     /* } */
+     285             : 
+     286             :     /* if (publish_world_tf_ && is_utm_based_ && is_world_origin_set_ && !is_world_static_tf_published_) { */
+     287             :     /*   publishWorldTf(msg->header.frame_id); */
+     288             :     /*   scope_timer.checkpoint("pub world tf"); */
+     289             :     /* } */
+     290             : 
+     291      343775 :     for (auto republisher : republishers_) {
+     292      132323 :       republishInFrame(msg, ch_->uav_name + "/" + republisher.first, republisher.second);
+     293             :     }
+     294             :   }
+     295             :   /*//}*/
+     296             : 
+     297             :   /*//{ callbackTfSourceAtt()*/
+     298      218228 :   void callbackTfSourceAtt(const geometry_msgs::QuaternionStamped::ConstPtr msg) {
+     299             : 
+     300      218228 :     if (!is_initialized_) {
+     301           0 :       return;
+     302             :     }
+     303             : 
+     304      654677 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::callbackTfSourceAtt", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     305             : 
+     306      218207 :     scope_timer.checkpoint("get msg");
+     307      218224 :     publishTfFromAtt(msg);
+     308             :   }
+     309             :   /*//}*/
+     310             : 
+     311             :   /* publishTfFromOdom() //{*/
+     312      211382 :   void publishTfFromOdom(const nav_msgs::OdometryConstPtr& odom) {
+     313             : 
+     314      422839 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishTfFromOdom", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     315             : 
+     316      211442 :     const tf2::Transform      tf       = Support::tf2FromPose(odom->pose.pose);
+     317      211412 :     const tf2::Transform      tf_inv   = tf.inverse();
+     318      211405 :     const geometry_msgs::Pose pose_inv = Support::poseFromTf2(tf_inv);
+     319             : 
+     320             :     /*//{ tf source origin */
+     321      211418 :     geometry_msgs::TransformStamped tf_msg;
+     322      211356 :     tf_msg.header.stamp         = odom->header.stamp;
+     323      211379 :     std::string origin_frame_id = custom_frame_id_enabled_ ? ch_->uav_name + "/" + custom_frame_id_ : odom->header.frame_id;
+     324      211391 :     std::string child_frame_id = custom_child_frame_id_enabled_ ? ch_->uav_name + "/" + custom_child_frame_id_ : ch_->frames.ns_fcu;
+     325      211413 :     if (is_inverted_) {
+     326             : 
+     327      211413 :       tf_msg.header.frame_id       = child_frame_id;
+     328      211463 :       tf_msg.child_frame_id        = origin_frame_id;
+     329      211350 :       tf_msg.transform.translation = Support::pointToVector3(pose_inv.position);
+     330      211401 :       tf_msg.transform.rotation    = pose_inv.orientation;
+     331             : 
+     332             :     } else {
+     333           0 :       tf_msg.header.frame_id       = origin_frame_id;
+     334           0 :       tf_msg.child_frame_id        = child_frame_id;
+     335           0 :       tf_msg.transform.translation = Support::pointToVector3(odom->pose.pose.position);
+     336           0 :       tf_msg.transform.rotation    = odom->pose.pose.orientation;
+     337             :     }
+     338             : 
+     339      211401 :     if (Support::noNans(tf_msg)) {
+     340             :       try {
+     341      211392 :         broadcaster_->sendTransform(tf_msg);
+     342             :       }
+     343           0 :       catch (...) {
+     344           0 :         ROS_ERROR("exception caught ");
+     345             :       }
+     346             : 
+     347      211461 :       if (tf_from_attitude_enabled_) {
+     348      210144 :         if (is_inverted_) {
+     349      210146 :           tf_msg.child_frame_id += "_att_only";
+     350             :         } else {
+     351           0 :           tf_msg.header.frame_id += "_att_only";
+     352             :         }
+     353             :         try {
+     354      210146 :           broadcaster_->sendTransform(tf_msg);
+     355             :         }
+     356           0 :         catch (...) {
+     357           0 :           ROS_ERROR("exception caught ");
+     358             :         }
+     359             :       }
+     360             :       /*//}*/
+     361             : 
+     362             :       /*//{ tf utm origin */
+     363             : 
+     364      211465 :       geometry_msgs::TransformStamped tf_utm_msg;
+     365      211462 :       if (is_utm_source_) {
+     366             :         
+     367      185787 :         if (!is_utm_origin_set_) {
+     368           0 :           ROS_INFO_THROTTLE(5.0, "[%s]: %s utm_origin initialization", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     369           0 :           return;
+     370             :         }
+     371             : 
+     372      185787 :         geometry_msgs::Pose pose_utm = odom->pose.pose;
+     373      185787 :         pose_utm.position.x += utm_origin_.x - first_msg_->pose.pose.position.x;
+     374      185787 :         pose_utm.position.y += utm_origin_.y - first_msg_->pose.pose.position.y;
+     375             : 
+     376      185787 :         if (sh_altitude_amsl_.hasMsg()) {
+     377      185781 :           pose_utm.position.z = sh_altitude_amsl_.getMsg()->amsl;
+     378             :         } else {
+     379           6 :           ROS_WARN_THROTTLE(5.0, "[%s]: AMSL altitude not available.", getPrintName().c_str());
+     380             :         }
+     381             : 
+     382      185787 :         tf_utm_msg.header.stamp    = odom->header.stamp;
+     383      185787 :         tf_utm_msg.header.frame_id = ns_utm_origin_parent_frame_id_;
+     384      185787 :         tf_utm_msg.child_frame_id  = ns_utm_origin_child_frame_id_;
+     385             : 
+     386      185787 :         tf2::Transform tf_utm;
+     387      185787 :         if (is_inverted_) {
+     388      185787 :           tf_utm = Support::tf2FromPose(pose_utm).inverse();
+     389             :         } else {
+     390           0 :           tf_utm = Support::tf2FromPose(pose_utm);
+     391             :         }
+     392      185787 :         tf_utm_msg.transform = Support::msgFromTf2(tf_utm);
+     393             : 
+     394             :         try {
+     395      185787 :           broadcaster_->sendTransform(tf_utm_msg);
+     396      185787 :           ROS_INFO_ONCE("[%s]: publishing utm_origin tf", getPrintName().c_str());
+     397             :         }
+     398           0 :         catch (...) {
+     399           0 :           ROS_ERROR("exception caught ");
+     400             :         }
+     401             :       }
+     402             :       /*//}*/
+     403             : 
+     404             :       /*//{ tf world origin */
+     405      211464 :       if (is_utm_source_) {
+     406      371573 :         geometry_msgs::TransformStamped tf_world_msg;
+     407      185787 :         tf_world_msg.header.stamp    = odom->header.stamp;
+     408      185787 :         tf_world_msg.header.frame_id = ns_world_origin_parent_frame_id_;
+     409      185787 :         tf_world_msg.child_frame_id  = ns_world_origin_child_frame_id_;
+     410             : 
+     411      185787 :         tf2::Transform tf_world;
+     412      185787 :         tf_world.setOrigin(tf2::Vector3(world_origin_.x, world_origin_.y, world_origin_.z));
+     413      185787 :         tf_world.setRotation(tf2::Quaternion(0, 0, 0, 1));
+     414             : 
+     415      185787 :         geometry_msgs::Pose pose_utm = odom->pose.pose;
+     416      185787 :         pose_utm.position.x += utm_origin_.x - first_msg_->pose.pose.position.x;
+     417      185787 :         pose_utm.position.y += utm_origin_.y - first_msg_->pose.pose.position.y;
+     418             : 
+     419      185787 :         tf2::Transform tf_utm;
+     420      185787 :         if (is_inverted_) {
+     421      185787 :           tf_utm = Support::tf2FromPose(pose_utm).inverse();
+     422             :         } else {
+     423           0 :           tf_utm = Support::tf2FromPose(pose_utm);
+     424             :         }
+     425             : 
+     426      185787 :         tf_world_msg.transform          = Support::msgFromTf2(tf_utm * tf_world);
+     427      185787 :         tf_world_msg.transform.rotation = pose_inv.orientation;
+     428             : 
+     429             :         try {
+     430      185787 :           broadcaster_->sendTransform(tf_world_msg);
+     431      185787 :           ROS_INFO_ONCE("[%s]: publishing world_origin tf", getPrintName().c_str());
+     432             :         }
+     433           0 :         catch (...) {
+     434           0 :           ROS_ERROR("exception caught ");
+     435             :         }
+     436             :       }
+     437             : 
+     438             :       /*//}*/
+     439             : 
+     440             :     } else {
+     441           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(),
+     442             :                         tf_msg.child_frame_id.c_str());
+     443             :     }
+     444      211463 :     ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s based on topic: %s", getPrintName().c_str(),
+     445             :                   tf_msg.header.frame_id.c_str(), tf_msg.child_frame_id.c_str(), full_topic_odom_.c_str());
+     446             :   }
+     447             :   /*//}*/
+     448             : 
+     449             :   /* publishTfFromAtt() //{*/
+     450      218217 :   void publishTfFromAtt(const geometry_msgs::QuaternionStampedConstPtr& msg) {
+     451             : 
+     452      654690 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishTfFromAtt", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     453             : 
+     454      436465 :     geometry_msgs::TransformStamped tf_msg;
+     455      218216 :     tf_msg.header.stamp = msg->header.stamp;
+     456             : 
+     457      218222 :     geometry_msgs::Pose pose;
+     458      218215 :     pose.position.x = 0.0;
+     459      218215 :     pose.position.y = 0.0;
+     460      218215 :     pose.position.z = 0.0;
+     461      218215 :     pose.orientation = msg->quaternion;
+     462      218218 :     if (is_inverted_) {
+     463             : 
+     464      218218 :       const tf2::Transform      tf       = Support::tf2FromPose(pose);
+     465      218214 :       const tf2::Transform      tf_inv   = tf.inverse();
+     466      218204 :       const geometry_msgs::Pose pose_inv = Support::poseFromTf2(tf_inv);
+     467             : 
+     468      218209 :       tf_msg.header.frame_id       = ch_->frames.ns_fcu;
+     469      218216 :       tf_msg.child_frame_id        = msg->header.frame_id;
+     470      218210 :       tf_msg.transform.translation = Support::pointToVector3(pose_inv.position);
+     471      218231 :       tf_msg.transform.rotation    = pose_inv.orientation;
+     472             : 
+     473             :     } else {
+     474           0 :       tf_msg.header.frame_id       = msg->header.frame_id;
+     475           0 :       tf_msg.child_frame_id        = ch_->frames.ns_fcu;
+     476           0 :       tf_msg.transform.translation = Support::pointToVector3(pose.position);
+     477           0 :       tf_msg.transform.rotation    = pose.orientation;
+     478             :     }
+     479             : 
+     480      218231 :     if (Support::noNans(tf_msg)) {
+     481             :       try {
+     482      218202 :         scope_timer.checkpoint("before pub");
+     483      218211 :         broadcaster_->sendTransform(tf_msg);
+     484      218238 :         scope_timer.checkpoint("after pub");
+     485             :       }
+     486           0 :       catch (...) {
+     487           0 :         ROS_ERROR("exception caught ");
+     488             :       }
+     489             :     } else {
+     490           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(),
+     491             :                         tf_msg.child_frame_id.c_str());
+     492             :     }
+     493      218235 :     ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s based on topic: %s", getPrintName().c_str(),
+     494             :                   tf_msg.header.frame_id.c_str(), tf_msg.child_frame_id.c_str(), full_topic_attitude_.c_str());
+     495      218240 :   }
+     496             :   /*//}*/
+     497             : 
+     498             :   /* publishLocalTf() //{*/
+     499           0 :   void publishLocalTf(const std::string& frame_id) {
+     500             : 
+     501           0 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishLocalTf", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     502             : 
+     503           0 :     geometry_msgs::TransformStamped tf_msg;
+     504           0 :     tf_msg.header.stamp = ros::Time::now();
+     505             : 
+     506           0 :     tf_msg.header.frame_id       = frame_id;
+     507           0 :     tf_msg.child_frame_id        = frame_id.substr(0, frame_id.find("_origin")) + "_local_origin";
+     508           0 :     tf_msg.transform.translation = Support::pointToVector3(first_msg_->pose.pose.position);
+     509           0 :     tf_msg.transform.rotation    = first_msg_->pose.pose.orientation;
+     510             : 
+     511           0 :     if (Support::noNans(tf_msg)) {
+     512             :       try {
+     513           0 :         static_broadcaster_.sendTransform(tf_msg);
+     514             :       }
+     515           0 :       catch (...) {
+     516           0 :         ROS_ERROR("exception caught ");
+     517             :       }
+     518             :     } else {
+     519           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(),
+     520             :                         tf_msg.child_frame_id.c_str());
+     521             :     }
+     522           0 :     ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s based on first message of: %s", getPrintName().c_str(),
+     523             :                   tf_msg.header.frame_id.c_str(), tf_msg.child_frame_id.c_str(), full_topic_odom_.c_str());
+     524           0 :     is_local_static_tf_published_ = true;
+     525           0 :   }
+     526             :   /*//}*/
+     527             : 
+     528             :   /* publishUtmTf() //{*/
+     529             :   void publishUtmTf(const std::string& frame_id) {
+     530             : 
+     531             :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishUtmTf", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     532             : 
+     533             :     geometry_msgs::TransformStamped tf_msg;
+     534             :     tf_msg.header.stamp = ros::Time::now();
+     535             : 
+     536             :     tf_msg.header.frame_id         = frame_id;
+     537             :     tf_msg.child_frame_id          = frame_id.substr(0, frame_id.find("_origin")) + "_utm_origin";
+     538             :     tf_msg.transform.translation.x = -utm_origin_.x;  // minus because inverse tf tree
+     539             :     tf_msg.transform.translation.y = -utm_origin_.y;  // minus because inverse tf tree
+     540             :     tf_msg.transform.translation.z = -utm_origin_.z;  // minus because inverse tf tree
+     541             :     tf_msg.transform.rotation.x    = 0;
+     542             :     tf_msg.transform.rotation.y    = 0;
+     543             :     tf_msg.transform.rotation.z    = 0;
+     544             :     tf_msg.transform.rotation.w    = 1;
+     545             : 
+     546             :     if (Support::noNans(tf_msg)) {
+     547             :       try {
+     548             :         static_broadcaster_.sendTransform(tf_msg);
+     549             :       }
+     550             :       catch (...) {
+     551             :         ROS_ERROR("exception caught ");
+     552             :       }
+     553             :     } else {
+     554             :       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(),
+     555             :                         tf_msg.child_frame_id.c_str());
+     556             :     }
+     557             :     ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s based on first message of: %s", getPrintName().c_str(),
+     558             :                   tf_msg.header.frame_id.c_str(), tf_msg.child_frame_id.c_str(), full_topic_odom_.c_str());
+     559             :     is_utm_static_tf_published_ = true;
+     560             :   }
+     561             :   /*//}*/
+     562             : 
+     563             :   /* publishWorldTf() //{*/
+     564             :   void publishWorldTf(const std::string& frame_id) {
+     565             : 
+     566             :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishWorldTf", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     567             : 
+     568             :     geometry_msgs::TransformStamped tf_msg;
+     569             :     tf_msg.header.stamp = ros::Time::now();
+     570             : 
+     571             :     tf_msg.header.frame_id         = frame_id;
+     572             :     tf_msg.child_frame_id          = frame_id.substr(0, frame_id.find("_origin")) + "_world_origin";
+     573             :     tf_msg.transform.translation.x = -(utm_origin_.x - world_origin_.x);  // minus because inverse tf tree
+     574             :     tf_msg.transform.translation.y = -(utm_origin_.y - world_origin_.y);  // minus because inverse tf tree
+     575             :     /* tf_msg.transform.translation.z = -(utm_origin_.z);                    // minus because inverse tf tree */
+     576             :     tf_msg.transform.translation.z = 0;
+     577             :     tf_msg.transform.rotation.x    = 0;
+     578             :     tf_msg.transform.rotation.y    = 0;
+     579             :     tf_msg.transform.rotation.z    = 0;
+     580             :     tf_msg.transform.rotation.w    = 1;
+     581             : 
+     582             :     if (Support::noNans(tf_msg)) {
+     583             :       try {
+     584             :         static_broadcaster_.sendTransform(tf_msg);
+     585             :       }
+     586             :       catch (...) {
+     587             :         ROS_ERROR("exception caught ");
+     588             :       }
+     589             :     } else {
+     590             :       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(),
+     591             :                         tf_msg.child_frame_id.c_str());
+     592             :     }
+     593             :     ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s based on first message of: %s", getPrintName().c_str(),
+     594             :                   tf_msg.header.frame_id.c_str(), tf_msg.child_frame_id.c_str(), full_topic_odom_.c_str());
+     595             :     is_world_static_tf_published_ = true;
+     596             :   }
+     597             :   /*//}*/
+     598             : 
+     599             :   /* republishInFrame() //{*/
+     600      132323 :   void republishInFrame(const nav_msgs::OdometryConstPtr& msg, const std::string& frame_id, mrs_lib::PublisherHandler<nav_msgs::Odometry>& ph) {
+     601             : 
+     602      264646 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::republishInFrame", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     603             : 
+     604      132323 :     nav_msgs::Odometry msg_out = *msg;
+     605      132323 :     msg_out.header.frame_id    = frame_id;
+     606             : 
+     607      132323 :     geometry_msgs::PoseStamped pose;
+     608      132323 :     pose.header = msg->header;
+     609      132323 :     pose.pose   = msg->pose.pose;
+     610             : 
+     611      132323 :     auto res = ch_->transformer->transformSingle(pose, frame_id);
+     612      132323 :     if (res) {
+     613      121688 :       msg_out.pose.pose = res->pose;
+     614      121688 :       ph.publish(msg_out);
+     615             :     } else {
+     616       10635 :       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());
+     617       10635 :       return;
+     618             :     }
+     619             :   }
+     620             : 
+     621             :   /*//}*/
+     622             : };
+     623             : 
+     624             : /*//}*/
+     625             : 
+     626             : }  // namespace mrs_uav_managers
+     627             : 
+     628             : #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..6608490567 --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.overview.html @@ -0,0 +1,177 @@ + + + + + + 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 + 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..a301c6c257cb62f330d093a863ccc5d916cffa52 GIT binary patch literal 2245 zcmV;$2s-zPP)r*aAD4rojC^p$r z1G~HCni@E!24@J(?bk`0yIw-ybDelZUETL>SG4?9;X`jIH4@&0G}78vE0>zLws~ z#-7qY&=mKyKhM|x+SB{+y1x*xI>o8y!Y=i^XK?)0x0*L+;98b!gSC<3OU^_G(f?o6a z7R(iM9e{qN!#zNLhtzd*uXok*5ldZnim#Bmv1-1n zA~Uh|KP0tt^i9yfqH6k*%i|i`9}%)S6D{BSCu!L8h1dT zztFQIGkMb%Ec8&MZ7%k*3WyhGChyO8eL_Bt?{h7Znp-jw;K9kv!zpmm0xJ}e&6Wy9 zF9O}|)&ftsZ|)?~T-foGdz;Vp2`Q3IX_hVzJlA!+0>gw8n@W9Aq|gK&Ds`n22y-e~ zu!qNj;z?mnbS)4roZ_k&o@!3Q$K{}+sRZV;D8=mTa^yvmi3Lqx#EedXJtR4aAjsV=}}G==y*i_Smd~~W`jS{0v3ML z4rBb!pKx4TcL5$XV-U;BuUt`lD^`TU!zl&MmIjABLe;dv~C5u~|V z7_dJB>cM!&Ey69Xq)3Gq+EMtiO({Ch$i(S9!|pYi&WQV)XKM2dfK@4eG8I3=fm+cc zb{MjioyXab9xt>^q;b5rzuca@DS*do1j-bdt>1J*Pu*UKsjWh(vJB`hY}x@m+7AqV zTL9Ar1+G5Ibx;&YB~JB7F!fn=;n51;6&SDvG!R&3*fIM;nMSccgM4R5m}6Sdodm$k znd9B_$u52{aEHJmkdl>D0^&x2`-1@YOz{n&8Ba(xr>Hv^uuN%L9;mBJaq6*4%ZBiz z_WEgWVsCjvz1iATTBI1Tv@RvC+4(YC)7W#3dPI))+&Q-mderF0Nq$=tvmp^i^a?Lb zOOhyz#w)1QqE~cD{o$qN>?f})$dz_6te^E2rfC`)3f0&{Oz3U+49}eDBOgdrtK2>h z*Xoh1rTEE^0`rtlS|4yo;gTiEa4pw_1_D*F2UGO*U-4`hJf@)!0ArRRYl>1muob!% zfTmcTL8&#u0E%oW3&=Fs_4hNJX07J1HJv^|80G@~808p(Pg&_S0xllPyUGQpNsjW*5_K}sx^NYuD3j795GJMR6!%vqrPLSJC7o_T4qmEy}z4pLH==$}X#D42;<_8Y{q^X;QACs;O zsH+?=Rra?YfNto4CKSA!`=-7dCP=@c@8(OuhbU&c@|Sp$PES7nfo|>Fd5%{yaLGWR z)0ox#zHXXz;eH(q459Z==Tj7q*iQ(hfywy&4k`B zG*`R_1k!xRLeM?Lm*xISk zY;zYGc5@FdxOr)eyWH+6!`@D@O>a01q#uRKaedtvH1zhYZ%Yc+;)K=Nv_$>`2pI)> TVv + + + + + + 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-12-17 22:35:29Functions: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()109
mrs_uav_managers::constraint_manager::ConstraintManager::onInit()109
mrs_uav_managers::constraint_manager::ConstraintManager::setConstraints(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)112
mrs_uav_managers::constraint_manager::ConstraintManager::timerDiagnostics(ros::TimerEvent const&)2426
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&)2941
mrs_uav_managers::constraint_manager::ConstraintManager::timerConstraintManagement(ros::TimerEvent const&)24770
+
+
+ + + +
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..a8f026f1ae --- /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-12-17 22:35:29Functions:88100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()109
mrs_uav_managers::constraint_manager::ConstraintManager::setConstraints(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)112
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&)2941
mrs_uav_managers::constraint_manager::ConstraintManager::timerDiagnostics(ros::TimerEvent const&)2426
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&)24770
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()109
+
+
+ + + +
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..c088852996 --- /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-12-17 22:35:29Functions: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         109 : void ConstraintManager::onInit() {
+     120             : 
+     121         109 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     122             : 
+     123         109 :   ros::Time::waitForValid();
+     124             : 
+     125         109 :   ROS_INFO("[ConstraintManager]: initializing");
+     126             : 
+     127             :   // | ------------------------- params ------------------------- |
+     128             : 
+     129         218 :   mrs_lib::ParamLoader param_loader(nh_, "ConstraintManager");
+     130             : 
+     131         218 :   std::string custom_config_path;
+     132         218 :   std::string platform_config_path;
+     133             : 
+     134         109 :   param_loader.loadParam("custom_config", custom_config_path);
+     135         109 :   param_loader.loadParam("platform_config", platform_config_path);
+     136             : 
+     137         109 :   if (custom_config_path != "") {
+     138         109 :     param_loader.addYamlFile(custom_config_path);
+     139             :   }
+     140             : 
+     141         109 :   if (platform_config_path != "") {
+     142         109 :     param_loader.addYamlFile(platform_config_path);
+     143             :   }
+     144             : 
+     145         109 :   param_loader.addYamlFileFromParam("private_config");
+     146         109 :   param_loader.addYamlFileFromParam("public_config");
+     147         109 :   param_loader.addYamlFileFromParam("public_constraints");
+     148             : 
+     149         218 :   const std::string yaml_prefix = "mrs_uav_managers/constraint_manager/";
+     150             : 
+     151             :   // params passed from the launch file are not prefixed
+     152         109 :   param_loader.loadParam("enable_profiler", _profiler_enabled_);
+     153             : 
+     154         109 :   param_loader.loadParam(yaml_prefix + "constraints", _constraint_names_);
+     155             : 
+     156         109 :   param_loader.loadParam(yaml_prefix + "estimator_types", _estimator_type_names_);
+     157             : 
+     158         109 :   param_loader.loadParam(yaml_prefix + "rate", _constraint_management_rate_);
+     159         109 :   param_loader.loadParam(yaml_prefix + "diagnostics_rate", _diagnostics_rate_);
+     160             : 
+     161         109 :   std::vector<std::string>::iterator it;
+     162             : 
+     163             :   // loading constraint names
+     164         436 :   for (it = _constraint_names_.begin(); it != _constraint_names_.end(); ++it) {
+     165             : 
+     166         327 :     ROS_INFO_STREAM("[ConstraintManager]: loading constraints '" << *it << "'");
+     167             : 
+     168         327 :     mrs_msgs::DynamicsConstraintsSrvRequest new_constraints;
+     169             : 
+     170         327 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/speed", new_constraints.constraints.horizontal_speed);
+     171         327 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/acceleration", new_constraints.constraints.horizontal_acceleration);
+     172         327 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/jerk", new_constraints.constraints.horizontal_jerk);
+     173         327 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/snap", new_constraints.constraints.horizontal_snap);
+     174             : 
+     175         327 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ascending/speed", new_constraints.constraints.vertical_ascending_speed);
+     176         327 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ascending/acceleration", new_constraints.constraints.vertical_ascending_acceleration);
+     177         327 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ascending/jerk", new_constraints.constraints.vertical_ascending_jerk);
+     178         327 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ascending/snap", new_constraints.constraints.vertical_ascending_snap);
+     179             : 
+     180         327 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/descending/speed", new_constraints.constraints.vertical_descending_speed);
+     181         327 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/descending/acceleration", new_constraints.constraints.vertical_descending_acceleration);
+     182         327 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/descending/jerk", new_constraints.constraints.vertical_descending_jerk);
+     183         327 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/descending/snap", new_constraints.constraints.vertical_descending_snap);
+     184             : 
+     185         327 :     param_loader.loadParam(yaml_prefix + *it + "/heading/speed", new_constraints.constraints.heading_speed);
+     186         327 :     param_loader.loadParam(yaml_prefix + *it + "/heading/acceleration", new_constraints.constraints.heading_acceleration);
+     187         327 :     param_loader.loadParam(yaml_prefix + *it + "/heading/jerk", new_constraints.constraints.heading_jerk);
+     188         327 :     param_loader.loadParam(yaml_prefix + *it + "/heading/snap", new_constraints.constraints.heading_snap);
+     189             : 
+     190         327 :     param_loader.loadParam(yaml_prefix + *it + "/angular_speed/roll", new_constraints.constraints.roll_rate);
+     191         327 :     param_loader.loadParam(yaml_prefix + *it + "/angular_speed/pitch", new_constraints.constraints.pitch_rate);
+     192         327 :     param_loader.loadParam(yaml_prefix + *it + "/angular_speed/yaw", new_constraints.constraints.yaw_rate);
+     193             : 
+     194             :     double tilt_deg;
+     195             : 
+     196         327 :     param_loader.loadParam(yaml_prefix + *it + "/tilt", tilt_deg);
+     197             : 
+     198         327 :     new_constraints.constraints.tilt = M_PI * (tilt_deg / 180.0);
+     199             : 
+     200         327 :     _constraints_.insert(std::pair<std::string, mrs_msgs::DynamicsConstraintsSrvRequest>(*it, new_constraints));
+     201             :   }
+     202             : 
+     203             :   // loading the allowed constraints lists
+     204         872 :   for (it = _estimator_type_names_.begin(); it != _estimator_type_names_.end(); ++it) {
+     205             : 
+     206         763 :     std::vector<std::string> temp_vector;
+     207         763 :     param_loader.loadParam(yaml_prefix + "allowed_constraints/" + *it, temp_vector);
+     208             : 
+     209         763 :     std::vector<std::string>::iterator it2;
+     210        2822 :     for (it2 = temp_vector.begin(); it2 != temp_vector.end(); ++it2) {
+     211        2059 :       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         763 :     _map_type_allowed_constraints_.insert(std::pair<std::string, std::vector<std::string>>(*it, temp_vector));
+     218             :   }
+     219             : 
+     220             :   // loading the default constraints
+     221         872 :   for (it = _estimator_type_names_.begin(); it != _estimator_type_names_.end(); ++it) {
+     222             : 
+     223         763 :     std::string temp_str;
+     224         763 :     param_loader.loadParam(yaml_prefix + "default_constraints/" + *it, temp_str);
+     225             : 
+     226         763 :     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         763 :     _map_type_default_constraints_.insert(std::pair<std::string, std::string>(*it, temp_str));
+     232             :   }
+     233             : 
+     234         109 :   ROS_INFO("[ConstraintManager]: done loading dynamic params");
+     235             : 
+     236         109 :   current_constraints_ = "";
+     237         109 :   last_estimator_name_ = "";
+     238             : 
+     239             :   // | ------------------------ services ------------------------ |
+     240             : 
+     241         109 :   service_server_set_constraints_ = nh_.advertiseService("set_constraints_in", &ConstraintManager::callbackSetConstraints, this);
+     242             : 
+     243         109 :   service_server_constraints_override_ = nh_.advertiseService("constraints_override_in", &ConstraintManager::callbackConstraintsOverride, this);
+     244             : 
+     245         109 :   sc_set_constraints_ = mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>(nh_, "set_constraints_out");
+     246             : 
+     247             :   // | ----------------------- subscribers ---------------------- |
+     248             : 
+     249         218 :   mrs_lib::SubscribeHandlerOptions shopts;
+     250         109 :   shopts.nh                 = nh_;
+     251         109 :   shopts.node_name          = "ConstraintManager";
+     252         109 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     253         109 :   shopts.threadsafe         = true;
+     254         109 :   shopts.autostart          = true;
+     255         109 :   shopts.queue_size         = 10;
+     256         109 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     257             : 
+     258         109 :   sh_estimation_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, "estimation_diagnostics_in");
+     259             : 
+     260             :   // | ----------------------- publishers ----------------------- |
+     261             : 
+     262         109 :   ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics>(nh_, "diagnostics_out", 10);
+     263             : 
+     264             :   // | ------------------------- timers ------------------------- |
+     265             : 
+     266         109 :   timer_constraint_management_ = nh_.createTimer(ros::Rate(_constraint_management_rate_), &ConstraintManager::timerConstraintManagement, this);
+     267         109 :   timer_diagnostics_           = nh_.createTimer(ros::Rate(_diagnostics_rate_), &ConstraintManager::timerDiagnostics, this);
+     268             : 
+     269             :   // | ------------------------ profiler ------------------------ |
+     270             : 
+     271         109 :   profiler_ = mrs_lib::Profiler(nh_, "ConstraintManager", _profiler_enabled_);
+     272             : 
+     273             :   // | ------------------- scope timer logger ------------------- |
+     274             : 
+     275         109 :   param_loader.loadParam(yaml_prefix + "scope_timer/enabled", scope_timer_enabled_);
+     276         327 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+     277         109 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+     278             : 
+     279             :   // | ----------------------- finish init ---------------------- |
+     280             : 
+     281         109 :   if (!param_loader.loadedSuccessfully()) {
+     282           0 :     ROS_ERROR("[ConstraintManager]: Could not load all parameters!");
+     283           0 :     ros::shutdown();
+     284             :   }
+     285             : 
+     286         109 :   is_initialized_ = true;
+     287             : 
+     288         109 :   ROS_INFO("[ConstraintManager]: initialized");
+     289             : 
+     290         109 :   ROS_DEBUG("[ConstraintManager]: debug output is enabled");
+     291         109 : }
+     292             : 
+     293             : //}
+     294             : 
+     295             : // --------------------------------------------------------------
+     296             : // |                           methods                          |
+     297             : // --------------------------------------------------------------
+     298             : 
+     299             : /* setConstraints() //{ */
+     300             : 
+     301         112 : bool ConstraintManager::setConstraints(std::string constraints_name) {
+     302             : 
+     303         112 :   std::map<std::string, mrs_msgs::DynamicsConstraintsSrvRequest>::iterator it;
+     304         112 :   it = _constraints_.find(constraints_name);
+     305             : 
+     306         112 :   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         224 :   mrs_msgs::DynamicsConstraintsSrv srv_call;
+     312             : 
+     313         112 :   srv_call.request = it->second;
+     314             : 
+     315         112 :   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         112 :   bool res = sc_set_constraints_.call(srv_call);
+     337             : 
+     338         112 :   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         112 :     if (srv_call.response.success) {
+     346             : 
+     347         112 :       mrs_lib::set_mutexed(mutex_current_constraints_, constraints_name, current_constraints_);
+     348         112 :       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       24770 : void ConstraintManager::timerConstraintManagement(const ros::TimerEvent& event) {
+     472             : 
+     473       24770 :   if (!is_initialized_) {
+     474        5746 :     return;
+     475             :   }
+     476             : 
+     477       49540 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerConstraintManagement", _constraint_management_rate_, 0.01, event);
+     478       49540 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ContraintManager::timerConstraintManagement", scope_timer_logger_, scope_timer_enabled_);
+     479             : 
+     480       24770 :   auto current_constraints = mrs_lib::get_mutexed(mutex_current_constraints_, current_constraints_);
+     481       24770 :   auto last_estimator_name = mrs_lib::get_mutexed(mutex_last_estimator_name_, last_estimator_name_);
+     482             : 
+     483       24770 :   if (!sh_estimation_diag_.hasMsg()) {
+     484        5746 :     return;
+     485             :   }
+     486             : 
+     487       19024 :   auto estimation_diagnostics = sh_estimation_diag_.getMsg();
+     488             : 
+     489             :   // | --- automatically set constraints when the state estimator changes -- |
+     490       19024 :   if (estimation_diagnostics->current_state_estimator != last_estimator_name_) {
+     491             : 
+     492         114 :     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         114 :     std::map<std::string, std::string>::iterator it;
+     496         114 :     it = _map_type_default_constraints_.find(estimation_diagnostics->current_state_estimator);
+     497             : 
+     498         114 :     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         114 :       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         109 :         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         109 :         if (setConstraints(it->second)) {
+     517             : 
+     518         109 :           last_estimator_name = estimation_diagnostics->current_state_estimator;
+     519             : 
+     520         109 :           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       19024 :   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       19024 :   mrs_lib::set_mutexed(mutex_last_estimator_name_, last_estimator_name, last_estimator_name_);
+     545             : }
+     546             : 
+     547             : //}
+     548             : 
+     549             : /* timerDiagnostics() //{ */
+     550             : 
+     551        2426 : void ConstraintManager::timerDiagnostics(const ros::TimerEvent& event) {
+     552             : 
+     553        2426 :   if (!is_initialized_) {
+     554         561 :     return;
+     555             :   }
+     556             : 
+     557        4852 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerDiagnostics", _diagnostics_rate_, 0.01, event);
+     558        4852 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ContraintManager::timerDiagnostics", scope_timer_logger_, scope_timer_enabled_);
+     559             : 
+     560        2426 :   if (!sh_estimation_diag_.hasMsg()) {
+     561         557 :     ROS_WARN_THROTTLE(10.0, "[ConstraintManager]: can not do constraint management, missing estimation diagnostics!");
+     562         557 :     return;
+     563             :   }
+     564             : 
+     565        1869 :   auto estimation_diagnostics = sh_estimation_diag_.getMsg();
+     566             : 
+     567        1869 :   auto current_constraints = mrs_lib::get_mutexed(mutex_current_constraints_, current_constraints_);
+     568             : 
+     569        1869 :   if (current_constraints == "") {  // this could happend just before timerConstraintManagement() finishes
+     570           4 :     return;
+     571             :   }
+     572             : 
+     573        1865 :   mrs_msgs::ConstraintManagerDiagnostics diagnostics;
+     574             : 
+     575        1865 :   diagnostics.stamp        = ros::Time::now();
+     576        1865 :   diagnostics.current_name = current_constraints;
+     577        1865 :   diagnostics.loaded       = _constraint_names_;
+     578             : 
+     579             :   // get the available constraints
+     580             :   {
+     581        1865 :     std::map<std::string, std::vector<std::string>>::iterator it;
+     582        1865 :     it = _map_type_allowed_constraints_.find(estimation_diagnostics->current_state_estimator);
+     583             : 
+     584        1865 :     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        1865 :       diagnostics.available = it->second;
+     589             :     }
+     590             :   }
+     591             : 
+     592             :   // get the current constraint values
+     593             :   {
+     594        1865 :     std::map<std::string, mrs_msgs::DynamicsConstraintsSrvRequest>::iterator it;
+     595        1865 :     it = _constraints_.find(current_constraints);
+     596             : 
+     597        1865 :     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        1865 :     diagnostics.current_values = it->second.constraints;
+     603             :   }
+     604             : 
+     605        1865 :   ph_diagnostics_.publish(diagnostics);
+     606             : }
+     607             : 
+     608             : //}
+     609             : 
+     610             : // --------------------------------------------------------------
+     611             : // |                          routines                          |
+     612             : // --------------------------------------------------------------
+     613             : 
+     614             : /* stringInVector() //{ */
+     615             : 
+     616        2941 : bool ConstraintManager::stringInVector(const std::string& value, const std::vector<std::string>& vector) {
+     617             : 
+     618        2941 :   if (std::find(vector.begin(), vector.end(), value) == vector.end()) {
+     619         110 :     return false;
+     620             :   } else {
+     621        2831 :     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         109 : 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..927fde6269 --- /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:23456741.3 %
Date:2024-12-17 22:35:29Functions:213167.7 %
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::HwApiVelocityHdgRateCmd_<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::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&)109
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&)508
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&)809
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&)1086
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&)1137
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&)1439
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&)1692
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&)2474
mrs_uav_managers::control_manager::RCChannelToRange(double, double, double)3140
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&)3976
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&)4022
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&)10838
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> >&, double const&)15083
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&)15083
mrs_uav_managers::control_manager::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)15635
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&)88456
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&)101632
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&)111347
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&)119592
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&)154139
mrs_uav_managers::control_manager::extractThrottle(mrs_uav_managers::Controller::ControlOutput const&)162023
+
+
+ + + +
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..8cb59a7385 --- /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:23456741.3 %
Date:2024-12-17 22:35:29Functions:213167.7 %
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&)1692
mrs_uav_managers::control_manager::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)15635
mrs_uav_managers::control_manager::extractThrottle(mrs_uav_managers::Controller::ControlOutput const&)162023
mrs_uav_managers::control_manager::getHighestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)0
mrs_uav_managers::control_manager::RCChannelToRange(double, double, double)3140
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&)154139
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&)10838
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&)15083
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&)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::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&)111347
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&)101632
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&)15083
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&)4022
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&)119592
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&)508
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&)809
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&)109
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&)1439
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&)88456
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&)3976
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&)1086
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&)2474
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&)1137
+
+
+ + + +
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..cddc4e9466 --- /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:23456741.3 %
Date:2024-12-17 22:35:29Functions:213167.7 %
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        1692 : std::optional<unsigned int> idxInVector(const std::string& str, const std::vector<std::string>& vec) {
+      12             : 
+      13        4793 :   for (unsigned int i = 0; i < vec.size(); i++) {
+      14        4789 :     if (str == vec[i]) {
+      15        1688 :       return {i};
+      16             :     }
+      17             :   }
+      18             : 
+      19           4 :   return {};
+      20             : }
+      21             : 
+      22             : //}
+      23             : 
+      24             : /* validateTrackerCommand() //{ */
+      25             : 
+      26      101632 : bool validateTrackerCommand(const std::optional<mrs_msgs::TrackerCommand>& msg, const std::string& node_name, const std::string& var_name) {
+      27             : 
+      28      101632 :   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      101632 :   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      101632 :   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      101632 :   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      101632 :   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      101632 :   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      101632 :   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      101632 :   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      101632 :   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      101632 :   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      101632 :   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      101632 :   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      101632 :   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      101632 :   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      101632 :   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      101632 :   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      101632 :   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      101632 :   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      101632 :   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      101632 :   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      101632 :   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      101632 :   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      101632 :   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      101632 :   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      101632 :   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         809 : bool validateVelocityReference(const mrs_msgs::VelocityReference& msg, const std::string& node_name, const std::string& var_name) {
+     237             : 
+     238             :   // check velocity
+     239             : 
+     240         809 :   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         809 :   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         809 :   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         809 :   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         809 :   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         809 :   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         809 :   return true;
+     271             : }
+     272             : 
+     273             : //}
+     274             : 
+     275             : /* validateReference() //{ */
+     276             : 
+     277       10838 : bool validateReference(const mrs_msgs::Reference& msg, const std::string& node_name, const std::string& var_name) {
+     278             : 
+     279             :   // check position
+     280             : 
+     281       10838 :   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       10838 :   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       10838 :   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       10838 :   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       10838 :   return true;
+     304             : }
+     305             : 
+     306             : //}
+     307             : 
+     308             : /* validateUavState() //{ */
+     309             : 
+     310      154139 : bool validateUavState(const mrs_msgs::UavState& msg, const std::string& node_name, const std::string& var_name) {
+     311             : 
+     312             :   // check position
+     313             : 
+     314      154139 :   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      154139 :   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      154139 :   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      154139 :   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      154139 :   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      154139 :   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      154139 :   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      154139 :   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      154139 :   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      154139 :   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      154139 :   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      154139 :   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      154139 :   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      154139 :   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      154139 :   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      154139 :   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      154139 :   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      154139 :   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      154139 :   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      154139 :   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      154139 :   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      154139 :   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      154139 :   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      154139 :   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      154139 :   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      154139 :   return true;
+     454             : }
+     455             : 
+     456             : //}
+     457             : 
+     458             : /* RCChannelToRange() //{ */
+     459             : 
+     460        3140 : double RCChannelToRange(double rc_value, double range, double deadband) {
+     461             : 
+     462        3140 :   double tmp_neg1_to_1 = (rc_value - 0.5) * 2.0;
+     463             : 
+     464        3140 :   if (tmp_neg1_to_1 > 1.0) {
+     465           0 :     tmp_neg1_to_1 = 1.0;
+     466        3140 :   } else if (tmp_neg1_to_1 < -1.0) {
+     467           0 :     tmp_neg1_to_1 = -1.0;
+     468             :   }
+     469             : 
+     470             :   // check the deadband
+     471        3140 :   if (tmp_neg1_to_1 < deadband && tmp_neg1_to_1 > -deadband) {
+     472        2423 :     return 0.0;
+     473             :   }
+     474             : 
+     475         717 :   if (tmp_neg1_to_1 > 0) {
+     476             : 
+     477         458 :     double tmp = (tmp_neg1_to_1 - deadband) / (1.0 - deadband);
+     478             : 
+     479         458 :     return range * tmp;
+     480             : 
+     481             :   } else {
+     482             : 
+     483         259 :     double tmp = (-tmp_neg1_to_1 - deadband) / (1.0 - deadband);
+     484             : 
+     485         259 :     return -range * tmp;
+     486             :   }
+     487             : 
+     488             :   return 0.0;
+     489             : }
+     490             : 
+     491             : //}
+     492             : 
+     493             : /* loadDetailedUavModelParams() //{ */
+     494             : 
+     495         109 : 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         218 :   mrs_lib::ParamLoader param_loader(nh, node_name);
+     499             : 
+     500         109 :   if (custom_config != "") {
+     501         109 :     param_loader.addYamlFile(custom_config);
+     502             :   }
+     503             : 
+     504         109 :   if (platform_config != "") {
+     505         109 :     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         109 :   bool detailed_loaded = true;
+     518             : 
+     519         109 :   detailed_loaded *= param_loader.loadParam("uav_mass", mass, 0.0);
+     520             : 
+     521         109 :   detailed_loaded *= param_loader.loadParam("model_params/arm_length", arm_length, 0.0);
+     522         109 :   detailed_loaded *= param_loader.loadParam("model_params/body_height", body_height, 0.0);
+     523             : 
+     524         109 :   detailed_loaded *= param_loader.loadParam("model_params/propulsion/force_constant", force_constant, 0.0);
+     525         109 :   detailed_loaded *= param_loader.loadParam("model_params/propulsion/torque_constant", torque_constant, 0.0);
+     526         109 :   detailed_loaded *= param_loader.loadParam("model_params/propulsion/prop_radius", prop_radius, 0.0);
+     527         109 :   detailed_loaded *= param_loader.loadParam("model_params/propulsion/rpm/min", rpm_min, 0.0);
+     528         109 :   detailed_loaded *= param_loader.loadParam("model_params/propulsion/rpm/max", rpm_max, 0.0);
+     529             : 
+     530         218 :   Eigen::MatrixXd allocation_matrix;
+     531             : 
+     532         109 :   detailed_loaded *= param_loader.loadMatrixDynamic("model_params/propulsion/allocation_matrix", allocation_matrix, Eigen::Matrix4d::Identity(), 4, -1);
+     533             : 
+     534         109 :   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         109 :     ROS_INFO("[%s]: detailed UAV model params successfully loaded.", node_name.c_str());
+     542             :   }
+     543             : 
+     544         109 :   int n_motors = allocation_matrix.cols();
+     545             : 
+     546         218 :   DetailedModelParams_t model_params;
+     547             : 
+     548         109 :   model_params.arm_length  = arm_length;
+     549         109 :   model_params.body_height = body_height;
+     550         109 :   model_params.prop_radius = prop_radius;
+     551             : 
+     552         109 :   Eigen::Matrix3d inertia_matrix;
+     553             : 
+     554         109 :   bool inertia_loaded = param_loader.loadMatrixStatic("model_params/inertia_matrix", inertia_matrix, Eigen::Matrix3d::Identity());
+     555             : 
+     556         109 :   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         109 :     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         109 :     model_params.inertia       = Eigen::Matrix3d::Zero();
+     568         109 :     model_params.inertia(0, 0) = mass * (3.0 * arm_length * arm_length + body_height * body_height) / 12.0;
+     569         109 :     model_params.inertia(1, 1) = mass * (3.0 * arm_length * arm_length + body_height * body_height) / 12.0;
+     570         109 :     model_params.inertia(2, 2) = (mass * arm_length * arm_length) / 2.0;
+     571             : 
+     572         109 :     ROS_INFO("[%s]: inertia computed form parameters:", node_name.c_str());
+     573         109 :     ROS_INFO_STREAM(model_params.inertia);
+     574             :   }
+     575             : 
+     576             :   // create the force-torque allocation matrix
+     577         109 :   model_params.force_torque_mixer = allocation_matrix;
+     578         109 :   model_params.force_torque_mixer.row(0) *= arm_length * force_constant;
+     579         109 :   model_params.force_torque_mixer.row(1) *= arm_length * force_constant;
+     580         109 :   model_params.force_torque_mixer.row(2) *= torque_constant * (3.0 * prop_radius) * force_constant;
+     581         109 :   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         218 :       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         551 :   for (int i = 0; i < n_motors; i++) {
+     594         442 :     alloc_tmp.block(i, 0, 1, 2).normalize();
+     595             :   }
+     596             : 
+     597             :   // the 3rd column (yaw)
+     598         551 :   for (int i = 0; i < n_motors; i++) {
+     599         442 :     if (alloc_tmp(i, 2) > 1e-2) {
+     600         221 :       alloc_tmp(i, 2) = 1.0;
+     601         221 :     } else if (alloc_tmp(i, 2) < -1e-2) {
+     602         221 :       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         551 :   for (int i = 0; i < n_motors; i++) {
+     610         442 :     alloc_tmp(i, 3) = 1.0;
+     611             :   }
+     612             : 
+     613         109 :   model_params.control_group_mixer = alloc_tmp;
+     614             : 
+     615         109 :   return model_params;
+     616             : }
+     617             : 
+     618             : //}
+     619             : 
+     620             : /* getLowestOutput() //{ */
+     621             : 
+     622       15635 : CONTROL_OUTPUT getLowestOuput(const ControlOutputModalities_t& outputs) {
+     623             : 
+     624       15635 :   if (outputs.actuators) {
+     625          16 :     return ACTUATORS_CMD;
+     626             :   }
+     627             : 
+     628       15619 :   if (outputs.control_group) {
+     629          16 :     return CONTROL_GROUP;
+     630             :   }
+     631             : 
+     632       15603 :   if (outputs.attitude_rate) {
+     633       15497 :     return ATTITUDE_RATE;
+     634             :   }
+     635             : 
+     636         106 :   if (outputs.attitude) {
+     637          16 :     return ATTITUDE;
+     638             :   }
+     639             : 
+     640          90 :   if (outputs.acceleration_hdg_rate) {
+     641          10 :     return ACCELERATION_HDG_RATE;
+     642             :   }
+     643             : 
+     644          80 :   if (outputs.acceleration_hdg) {
+     645          10 :     return ACCELERATION_HDG;
+     646             :   }
+     647             : 
+     648          70 :   if (outputs.velocity_hdg_rate) {
+     649          40 :     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      162023 : std::optional<double> extractThrottle(const Controller::ControlOutput& control_output) {
+     707             : 
+     708      162023 :   if (!control_output.control_output) {
+     709       24451 :     return {};
+     710             :   }
+     711             : 
+     712      275144 :   return std::visit(HwApiCmdExtractThrottleVisitor(), control_output.control_output.value());
+     713             : }
+     714             : 
+     715             : //}
+     716             : 
+     717             : // | -------------- validation of HW api commands ------------- |
+     718             : 
+     719             : /* validateControlOutput() //{ */
+     720             : 
+     721      111347 : 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      111347 :   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      111347 :   std::variant<ControlOutputModalities_t> output_modalities_var{output_modalities};
+     730      222694 :   std::variant<std::string>               node_name_var{node_name};
+     731      111347 :   std::variant<std::string>               var_name_var{var_name};
+     732             : 
+     733      111347 :   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        4022 : bool validateHwApiActuatorCmd(const mrs_msgs::HwApiActuatorCmd& msg, const std::string& node_name, const std::string& var_name) {
+     741             : 
+     742       20110 :   for (size_t i = 0; i < msg.motors.size(); i++) {
+     743       16088 :     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        4022 :   return true;
+     750             : }
+     751             : 
+     752             : //}
+     753             : 
+     754             : /* validateHwApiControlGroupCmd() //{ */
+     755             : 
+     756        3976 : bool validateHwApiControlGroupCmd(const mrs_msgs::HwApiControlGroupCmd& msg, const std::string& node_name, const std::string& var_name) {
+     757             : 
+     758        3976 :   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        3976 :   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        3976 :   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        3976 :   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        3976 :   return true;
+     779             : }
+     780             : 
+     781             : //}
+     782             : 
+     783             : /* validateHwApiAttitudeCmd() //{ */
+     784             : 
+     785      119592 : bool validateHwApiAttitudeCmd(const mrs_msgs::HwApiAttitudeCmd& msg, const std::string& node_name, const std::string& var_name) {
+     786             : 
+     787             :   // check the orientation
+     788             : 
+     789      119592 :   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      119592 :   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      119592 :   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      119592 :   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      119592 :   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      119592 :   return true;
+     817             : }
+     818             : 
+     819             : //}
+     820             : 
+     821             : /* validateHwApiAttitudeRateCmd() //{ */
+     822             : 
+     823       88456 : 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       88456 :   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       88456 :   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       88456 :   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       88456 :   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       88456 :   return true;
+     850             : }
+     851             : 
+     852             : //}
+     853             : 
+     854             : /* validateHwApiAccelerationHdgRateCmd() //{ */
+     855             : 
+     856        1137 : bool validateHwApiAccelerationHdgRateCmd(const mrs_msgs::HwApiAccelerationHdgRateCmd& msg, const std::string& node_name, const std::string& var_name) {
+     857             : 
+     858             :   // | ----------------- check the acceleration ----------------- |
+     859             : 
+     860        1137 :   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        1137 :   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        1137 :   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        1137 :   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        1137 :   return true;
+     883             : }
+     884             : 
+     885             : //}
+     886             : 
+     887             : /* validateHwApiAccelerationHdgCmd() //{ */
+     888             : 
+     889        1086 : bool validateHwApiAccelerationHdgCmd(const mrs_msgs::HwApiAccelerationHdgCmd& msg, const std::string& node_name, const std::string& var_name) {
+     890             : 
+     891             :   // | ----------------- check the acceleration ----------------- |
+     892             : 
+     893        1086 :   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        1086 :   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        1086 :   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        1086 :   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        1086 :   return true;
+     916             : }
+     917             : 
+     918             : //}
+     919             : 
+     920             : /* validateHwApiVelocityHdgRateCmd() //{ */
+     921             : 
+     922        2474 : bool validateHwApiVelocityHdgRateCmd(const mrs_msgs::HwApiVelocityHdgRateCmd& msg, const std::string& node_name, const std::string& var_name) {
+     923             : 
+     924             :   // | ----------------- check the velocity ----------------- |
+     925             : 
+     926        2474 :   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        2474 :   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        2474 :   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        2474 :   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        2474 :   return true;
+     949             : }
+     950             : 
+     951             : //}
+     952             : 
+     953             : /* validateHwApiVelocityHdgCmd() //{ */
+     954             : 
+     955        1439 : bool validateHwApiVelocityHdgCmd(const mrs_msgs::HwApiVelocityHdgCmd& msg, const std::string& node_name, const std::string& var_name) {
+     956             : 
+     957             :   // | ----------------- check the velocity ----------------- |
+     958             : 
+     959        1439 :   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        1439 :   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        1439 :   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        1439 :   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        1439 :   return true;
+     982             : }
+     983             : 
+     984             : //}
+     985             : 
+     986             : /* validateHwApiPositionHdgCmd() //{ */
+     987             : 
+     988         508 : bool validateHwApiPositionCmd(const mrs_msgs::HwApiPositionCmd& msg, const std::string& node_name, const std::string& var_name) {
+     989             : 
+     990             :   // | ----------------- check the position ----------------- |
+     991             : 
+     992         508 :   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         508 :   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         508 :   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         508 :   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         508 :   return true;
+    1015             : }
+    1016             : 
+    1017             : //}
+    1018             : 
+    1019             : // | ------------ initialization of HW api commands ----------- |
+    1020             : 
+    1021             : /* initializeDefaultOutput() //{ */
+    1022             : 
+    1023       15083 : 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       15083 :   CONTROL_OUTPUT lowest_output = getLowestOuput(possible_outputs);
+    1027             : 
+    1028       15083 :   Controller::HwApiOutputVariant output;
+    1029             : 
+    1030       15083 :   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       15083 :     case ATTITUDE_RATE: {
+    1040       15083 :       output = mrs_msgs::HwApiAttitudeRateCmd();
+    1041       15083 :       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           0 :     case VELOCITY_HDG_RATE: {
+    1056           0 :       output = mrs_msgs::HwApiVelocityHdgRateCmd();
+    1057           0 :       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       30166 :   std::variant<mrs_msgs::UavState> uav_state_var{uav_state};
+    1070       15083 :   std::variant<double>             min_throttle_var{min_throttle};
+    1071       15083 :   std::variant<double>             n_motors_var{n_motors};
+    1072             : 
+    1073       15083 :   std::visit(HwApiInitializeVisitor(), output, uav_state_var, min_throttle_var, n_motors_var);
+    1074             : 
+    1075       30166 :   return output;
+    1076             : }
+    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       15083 : void initializeHwApiCmd(mrs_msgs::HwApiAttitudeRateCmd& msg, const double& min_throttle) {
+    1110             : 
+    1111       15083 :   msg.stamp = ros::Time::now();
+    1112             : 
+    1113       15083 :   msg.body_rate.x = 0;
+    1114       15083 :   msg.body_rate.y = 0;
+    1115       15083 :   msg.body_rate.z = 0;
+    1116       15083 :   msg.throttle    = min_throttle;
+    1117       15083 : }
+    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           0 : void initializeHwApiCmd(mrs_msgs::HwApiVelocityHdgRateCmd& msg, const mrs_msgs::UavState& uav_state) {
+    1172             : 
+    1173           0 :   msg.header.frame_id = uav_state.header.frame_id;
+    1174           0 :   msg.header.stamp    = ros::Time::now();
+    1175             : 
+    1176           0 :   msg.velocity.x   = 0;
+    1177           0 :   msg.velocity.y   = 0;
+    1178           0 :   msg.velocity.z   = 0;
+    1179           0 :   msg.heading_rate = 0;
+    1180           0 : }
+    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..1cf22a17821c30b523d33c660c1d7cea6d6beba0 GIT binary patch literal 2424 zcmV-;35WKHP)5)@ zZU63*!sL0j2U6E4_X=GrB@VBl5c9>hX_@uq3_g+9kCE6Oqx{Ca%=5fV)1*1g2f|Tw zuoFx;$Bk03NOnD@NEBiDp^*|2x~56E%Sdgw5uRy|uRgawmpLxWw7@nmfwqlsj+t#7 z4@MOre82x}TerWT?T@_LE>`oSB;2moH{Kra6on?t)A(>D9{O$c6drT3xO$9|Wc#Ih z7)+~Rtl^dlLbp;6gJ~6vHQZ7$H=v%Ng1S%-gJ~6vGTee|zJq!qt8t=we!G%lFs*`N z!(}jCf-y^lXVs-bU@zOngv4N41!D|1<13l!i6mgC>iM)b)}c7n#aIUoHvp(xFx`%f zojx+Ylc}EY^h#CF`rEM5lHLX zN*C_RU|IzOQg#_%I-#BoV2A2qFbw7kV0ijqFr9)CJ*PPmd(P;(@)H_Go(J-u&Mt6NnNOi!L$m-Gu#5n5?nn~Uw+MC+6BXR0g$!??x#*N z^RNCVBUMfX7Q<3j`I+`AKgSWstmz^pL2CjTdt6uifv(56Gt&1wb}3W$xpt12Y1WIl z>70V29*ax<1l^TbFKHtr`T`vblo6}ylwlMS%;aDf1|mM00bw3TVXp~7BFD!zRSp>= zPSB%^+V!x-I$1vIkc+y6#f{?eBA)!NP%*9cqn8EZb71NXPRPlzHpSY=Z9MiVS@?S{ znn^e@d^cZ=5cF+Zh_GIeN(dQ|@hD_9pP`r*d6iC1qZq;zPe6$C%J7pqXs5&OdxE{_=g$YMz3@7!_lc{j3|5>Wc zGqO)-q=Q39A8XU}>OVqwg5xqPNt1*cQt;VpI$^Gi+6!X?mBrE7Pn7gRyeKK1BTTw{ z^g*n!yMoNjsuD2zUyz=0(>Q-9M_Q}3@zH~=vJ?AOS6xbnowJ@+e8^j$IuD=jTDpMa zws1_{W#O!MWokbe1GB2G4-T*0@2A71p?BVDthpJV3sPb7Vd~ z5DMHt(fS)j2w@sxNf0_CL)GJJP`5sf@jkVt#LxMQUsFIFYW?{-?lsKrHqp-**&_L2 zH5;`yh{4*0+Cdm;_MC!9Ea(K9($;W#TOfHDM$u>?CiZEXW|fVKR$u|zne2)$L|DbS z289AuM-CXB@7+nG^PDw`2~Rfd1=~R$Q@B$2JeSKct}Dec-lZXY>&ftgCy_ToTE@3h zz5l^@D6$vNw#UT4)JeMd!O_j%0vU;2gBuVyVPAKAO1<1ctJ%h6SN+S0J3f|KBDrrn zMz^Fjb9=ayEF)P)JgZ=3Omtmak8xc(dc<^DUq)=wgfb#s*VkjrB0U+gX}GS8h($Uw zqFvXS;s~D5+k{lEmYrO1fwTrpM2JMiCoBB`3e+5m(s+wF-CK%d-6A@Olt9MBLk;x{ z{lpThj*M6;xxS21O?omSU4NrsQdpCojB!oYm9Y<>LDKw=9>4t9P9HLodhVs>)$lSI zu&3TBi4F3_Y*_I<*Wzrpj{B~2clXf6k6!l>IM!SD9GVNglXcGka-Z75%-N|+W>(75 ze05JDogZ$JxaA%kGop8Wy|BukyoKDtd-p=Q>(Ko`mr&yke3m5CADzl_p+a`zT)-8I zY%C7Snf;%n+2_8%6u!N538qv*KHRj}3(;uL&nv>TFZ?noebeXcOIakrS?9bun&b=; z$;v*zKBYlSHIcxqO@WCfa!Fv;zox+SUiI>}L8{G36C7fxSktB3m%uO>2J;0l0bLndAvF582`+W&DgJ~5E2hs$X8wb+sS=vZ8{P5wKhhkrFq5U+$KU><+ zx+i3kUAQZQX%!3zq=nx7&F_Ff3CZmnf%NLlu3#VyEK)g;7PChN(@0bU~5h1)gX5revHoBcmhsCxY zLgG*#>-S7ctiD+eM=cjx1!4CYeflbK(oT4ncXlvJNN_Il^Whg|JPK1m#t~L)Lq|rQ zfr=T8Pe>VS;$bPP$3#uSM#dT)@90?ndGWutjEFAdGOj_wYBJ&{x?7MDwaA!^hi+=h z*s#ykIX@GhUt#R=KlS4piO>ssv}hJM7iVX4yHlI)1+>Q1h|yiypz z55;fxUkZYp(ikE*Mg1B243*<9%;&R8uF?9tp)#Yv?&*64^6_*c-TRA1ao#MVM&X&o zZ;e6(+|4RTT@wjcGDkZt#0mdT>xmu7UqEs#_h9S-3wuT*^@+mHbE1M@H9}D@5<^3u zkcf2ST_&Wz-%m)2)#^$ZBYeMCyzkBqt-*ekGe z0gkCnKt;lY6ZURq=Zp+BGhk$$nOSP9S%Z)&Bgb%@BbD$!-gEuhxf~>4A*9O4&V_TN q5|($ar!4TPpOPrR-{Tma`~Cq?hLt&YJF!Cm0000 + + + + + + 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:23456741.3 %
Date:2024-12-17 22:35:29Functions: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
common.cpp +
41.3%41.3%
+ 		41.3 %234 / 56767.7 %21 / 31
<unnamed>41.3 %234 / 56767.7 %21 / 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..9e46ca6d71 --- /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:23456741.3 %
Date:2024-12-17 22:35:29Functions: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
common.cpp +
41.3%41.3%
+ 		41.3 %234 / 56767.7 %21 / 31
<unnamed>41.3 %234 / 56767.7 %21 / 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..232a824212 --- /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:23456741.3 %
Date:2024-12-17 22:35:29Functions: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
common.cpp +
41.3%41.3%
+ 		41.3 %234 / 56767.7 %21 / 31
<unnamed>41.3 %234 / 56767.7 %21 / 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..c17164ede4 --- /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:23456741.3 %
Date:2024-12-17 22:35:29Functions: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
common.cpp +
41.3%41.3%
+ 		41.3 %234 / 56767.7 %21 / 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..6a0ed03790 --- /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:23456741.3 %
Date:2024-12-17 22:35:29Functions: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
common.cpp +
41.3%41.3%
+ 		41.3 %234 / 56767.7 %21 / 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..8972e4e17e --- /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:23456741.3 %
Date:2024-12-17 22:35:29Functions: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
common.cpp +
41.3%41.3%
+ 		41.3 %234 / 56767.7 %21 / 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..5940d1898e --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.func-sort-c.html @@ -0,0 +1,528 @@ + + + + + + + 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:2549370268.9 %
Date:2024-12-17 22:35:29Functions:9511284.8 %
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::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::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&)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::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::callbackUseSafetyArea(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<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::isPathToPointInSafetyArea2d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::ControlManager::callbackTransformReferenceArray(mrs_msgs::TransformReferenceArraySrvRequest_<std::allocator<void> >&, mrs_msgs::TransformReferenceArraySrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackHover(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::callbackGotoFcu(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::callbackTransformPose(mrs_msgs::TransformPoseSrvRequest_<std::allocator<void> >&, mrs_msgs::TransformPoseSrvResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::callbackReferenceTopic(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>)1
mrs_uav_managers::control_manager::ControlManager::stopTrajectoryTracking[abi:cxx11]()1
mrs_uav_managers::control_manager::ControlManager::callbackTransformVector3(mrs_msgs::TransformVector3SrvRequest_<std::allocator<void> >&, mrs_msgs::TransformVector3SrvResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::resumeTrajectoryTracking[abi:cxx11]()1
mrs_uav_managers::control_manager::ControlManager::callbackTransformReference(mrs_msgs::TransformReferenceSrvRequest_<std::allocator<void> >&, mrs_msgs::TransformReferenceSrvResponse_<std::allocator<void> >&)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::hover[abi:cxx11]()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::callbackGotoAltitude(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)2
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> >&)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::callbackValidateReferenceArray(mrs_msgs::ValidateReferenceArrayRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceArrayResponse_<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::callbackSetHeadingRelative(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)3
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::callbackSetHeading(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)4
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<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::failsafe[abi:cxx11]()7
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::changeLandingState(mrs_uav_managers::control_manager::LandingStates_t)12
mrs_uav_managers::control_manager::ControlManager::odometryCallbacksSrv(bool)15
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> >&)25
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::ungripSrv()30
mrs_uav_managers::control_manager::ControlManager::setReference[abi:cxx11](mrs_msgs::ReferenceStamped_<std::allocator<void> >)65
mrs_uav_managers::control_manager::ControlManager::callbackVelocityReferenceTopic(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>)92
mrs_uav_managers::control_manager::ControlManager::callbackEmergencyReference(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)101
mrs_uav_managers::control_manager::ControlManager::setCallbacks(bool)102
mrs_uav_managers::control_manager::ControlManager::callbackEnableCallbacks(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)102
mrs_uav_managers::control_manager::ControlManager::bumperGetSectorId(double const&, double const&, double const&)104
(anonymous namespace)::ProxyExec0::ProxyExec0()109
mrs_uav_managers::control_manager::ControlManager::initialize()109
mrs_uav_managers::control_manager::ControlManager::preinitialize()109
mrs_uav_managers::control_manager::ControlManager::setConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)109
mrs_uav_managers::control_manager::ControlManager::onInit()109
mrs_uav_managers::control_manager::ControlManager::callbackToggleOutput(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)110
mrs_uav_managers::control_manager::ControlManager::callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&, mrs_msgs::DynamicsConstraintsSrvResponse_<std::allocator<void> >&)112
mrs_uav_managers::control_manager::ControlManager::toggleOutput(bool const&)131
mrs_uav_managers::control_manager::ControlManager::escalatingFailsafe[abi:cxx11]()147
mrs_uav_managers::control_manager::ControlManager::timerHwApiCapabilities(ros::TimerEvent const&)185
mrs_uav_managers::control_manager::ControlManager::callbackSwitchTracker(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)214
mrs_uav_managers::control_manager::ControlManager::callbackSwitchController(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)215
mrs_uav_managers::control_manager::ControlManager::initializeControlOutput()228
mrs_uav_managers::control_manager::ControlManager::switchTracker(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)246
mrs_uav_managers::control_manager::ControlManager::switchController(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)247
mrs_uav_managers::control_manager::ControlManager::bumperPushFromObstacle()276
mrs_uav_managers::control_manager::ControlManager::setConstraintsToTrackers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)387
mrs_uav_managers::control_manager::ControlManager::setConstraintsToControllers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)431
mrs_uav_managers::control_manager::ControlManager::timerEland(ros::TimerEvent const&)482
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)545
mrs_uav_managers::control_manager::ControlManager::getMass()560
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)655
mrs_uav_managers::control_manager::ControlManager::callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::VelocityReferenceStampedSrvResponse_<std::allocator<void> >&)717
mrs_uav_managers::control_manager::ControlManager::setVelocityReference[abi:cxx11](mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)809
mrs_uav_managers::control_manager::ControlManager::velocityReferenceToReference(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)809
mrs_uav_managers::control_manager::ControlManager::isPathToPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)879
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)1842
mrs_uav_managers::control_manager::ControlManager::timerBumper(ros::TimerEvent const&)2484
mrs_uav_managers::control_manager::ControlManager::timerFailsafe(ros::TimerEvent const&)9716
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference2d(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<std::allocator<void> >&)10071
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea2d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)10181
mrs_uav_managers::control_manager::ControlManager::getMaxZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)13910
mrs_uav_managers::control_manager::ControlManager::getMinZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)13913
mrs_uav_managers::control_manager::ControlManager::timerStatus(ros::TimerEvent const&)19436
mrs_uav_managers::control_manager::ControlManager::publishDiagnostics()19545
mrs_uav_managers::control_manager::ControlManager::isFlyingNormally()19826
mrs_uav_managers::control_manager::ControlManager::callbackRC(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>)24855
mrs_uav_managers::control_manager::ControlManager::timerJoystick(ros::TimerEvent const&)68187
mrs_uav_managers::control_manager::ControlManager::callbackGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)79573
mrs_uav_managers::control_manager::ControlManager::enforceControllersConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)141539
mrs_uav_managers::control_manager::ControlManager::updateTrackers()142109
mrs_uav_managers::control_manager::ControlManager::asyncControl()151343
mrs_uav_managers::control_manager::ControlManager::updateControllers(mrs_msgs::UavState_<std::allocator<void> > const&)151825
mrs_uav_managers::control_manager::ControlManager::publishControlReferenceOdom(std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, mrs_uav_managers::Controller::ControlOutput const&)151825
mrs_uav_managers::control_manager::ControlManager::publish()151825
mrs_uav_managers::control_manager::ControlManager::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)176221
mrs_uav_managers::control_manager::ControlManager::timerSafety(ros::TimerEvent const&)351996
mrs_uav_managers::control_manager::ControlManager::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)452443
+
+
+ + + +
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..e54a7d6803 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.func.html @@ -0,0 +1,528 @@ + + + + + + + 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:2549370268.9 %
Date:2024-12-17 22:35:29Functions:9511284.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()109
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)655
mrs_uav_managers::control_manager::ControlManager::callbackRC(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>)24855
mrs_uav_managers::control_manager::ControlManager::initialize()109
mrs_uav_managers::control_manager::ControlManager::isOffboard()18
mrs_uav_managers::control_manager::ControlManager::timerEland(ros::TimerEvent const&)482
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&)2484
mrs_uav_managers::control_manager::ControlManager::timerSafety(ros::TimerEvent const&)351996
mrs_uav_managers::control_manager::ControlManager::timerStatus(ros::TimerEvent const&)19436
mrs_uav_managers::control_manager::ControlManager::asyncControl()151343
mrs_uav_managers::control_manager::ControlManager::callbackGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)79573
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)102
mrs_uav_managers::control_manager::ControlManager::setReference[abi:cxx11](mrs_msgs::ReferenceStamped_<std::allocator<void> >)65
mrs_uav_managers::control_manager::ControlManager::toggleOutput(bool const&)131
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> >&)1
mrs_uav_managers::control_manager::ControlManager::preinitialize()109
mrs_uav_managers::control_manager::ControlManager::switchTracker(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)246
mrs_uav_managers::control_manager::ControlManager::timerFailsafe(ros::TimerEvent const&)9716
mrs_uav_managers::control_manager::ControlManager::timerJoystick(ros::TimerEvent const&)68187
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&)109
mrs_uav_managers::control_manager::ControlManager::timerPirouette(ros::TimerEvent const&)0
mrs_uav_managers::control_manager::ControlManager::updateTrackers()142109
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> >&)1
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&)0
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>)176221
mrs_uav_managers::control_manager::ControlManager::isFlyingNormally()19826
mrs_uav_managers::control_manager::ControlManager::switchController(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)247
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&)151825
mrs_uav_managers::control_manager::ControlManager::callbackSetHeading(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)4
mrs_uav_managers::control_manager::ControlManager::changeLandingState(mrs_uav_managers::control_manager::LandingStates_t)12
mrs_uav_managers::control_manager::ControlManager::escalatingFailsafe[abi:cxx11]()147
mrs_uav_managers::control_manager::ControlManager::publishDiagnostics()19545
mrs_uav_managers::control_manager::ControlManager::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)452443
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> >&)2
mrs_uav_managers::control_manager::ControlManager::callbackGotoRelative(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)25
mrs_uav_managers::control_manager::ControlManager::callbackToggleOutput(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)110
mrs_uav_managers::control_manager::ControlManager::odometryCallbacksSrv(bool)15
mrs_uav_managers::control_manager::ControlManager::setVelocityReference[abi:cxx11](mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)809
mrs_uav_managers::control_manager::ControlManager::callbackSwitchTracker(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)214
mrs_uav_managers::control_manager::ControlManager::callbackTransformPose(mrs_msgs::TransformPoseSrvRequest_<std::allocator<void> >&, mrs_msgs::TransformPoseSrvResponse_<std::allocator<void> >&)1
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&)10181
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)1842
mrs_uav_managers::control_manager::ControlManager::bumperPushFromObstacle()276
mrs_uav_managers::control_manager::ControlManager::callbackReferenceTopic(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>)1
mrs_uav_managers::control_manager::ControlManager::callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&, mrs_msgs::DynamicsConstraintsSrvResponse_<std::allocator<void> >&)112
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&)185
mrs_uav_managers::control_manager::ControlManager::callbackEnableCallbacks(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)102
mrs_uav_managers::control_manager::ControlManager::getNextEscFailsafeState()8
mrs_uav_managers::control_manager::ControlManager::initializeControlOutput()228
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> >&)2
mrs_uav_managers::control_manager::ControlManager::callbackSwitchController(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)215
mrs_uav_managers::control_manager::ControlManager::callbackTransformVector3(mrs_msgs::TransformVector3SrvRequest_<std::allocator<void> >&, mrs_msgs::TransformVector3SrvResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::resumeTrajectoryTracking[abi:cxx11]()1
mrs_uav_managers::control_manager::ControlManager::setConstraintsToTrackers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)387
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<std::allocator<void> >&)4
mrs_uav_managers::control_manager::ControlManager::callbackEmergencyReference(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)101
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> >&)3
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> >&)1
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> >&)10071
mrs_uav_managers::control_manager::ControlManager::isPathToPointInSafetyArea2d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)0
mrs_uav_managers::control_manager::ControlManager::isPathToPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)879
mrs_uav_managers::control_manager::ControlManager::publishControlReferenceOdom(std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, mrs_uav_managers::Controller::ControlOutput const&)151825
mrs_uav_managers::control_manager::ControlManager::setConstraintsToControllers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)431
mrs_uav_managers::control_manager::ControlManager::velocityReferenceToReference(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)809
mrs_uav_managers::control_manager::ControlManager::enforceControllersConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)141539
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::callbackValidateReferenceArray(mrs_msgs::ValidateReferenceArrayRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceArrayResponse_<std::allocator<void> >&)2
mrs_uav_managers::control_manager::ControlManager::callbackVelocityReferenceTopic(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>)92
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::callbackTransformReferenceArray(mrs_msgs::TransformReferenceArraySrvRequest_<std::allocator<void> >&, mrs_msgs::TransformReferenceArraySrvResponse_<std::allocator<void> >&)0
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> >&)717
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]()1
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()109
mrs_uav_managers::control_manager::ControlManager::getMass()560
mrs_uav_managers::control_manager::ControlManager::getMaxZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)13910
mrs_uav_managers::control_manager::ControlManager::getMinZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)13913
mrs_uav_managers::control_manager::ControlManager::publish()151825
mrs_uav_managers::control_manager::ControlManager::elandSrv()8
mrs_uav_managers::control_manager::ControlManager::failsafe[abi:cxx11]()7
mrs_uav_managers::control_manager::ControlManager::ungripSrv()30
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)545
+
+
+ + + +
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..32921c5182 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.html @@ -0,0 +1,9009 @@ + + + + + + + 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:2549370268.9 %
Date:2024-12-17 22:35:29Functions:9511284.8 %
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/ValidateReferenceArray.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/ReferenceArray.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/TransformReferenceArraySrv.h>
+      97             : #include <mrs_msgs/TransformReferenceArraySrvRequest.h>
+      98             : #include <mrs_msgs/TransformReferenceArraySrvResponse.h>
+      99             : 
+     100             : #include <mrs_msgs/TransformPoseSrv.h>
+     101             : #include <mrs_msgs/TransformPoseSrvRequest.h>
+     102             : #include <mrs_msgs/TransformPoseSrvResponse.h>
+     103             : 
+     104             : #include <mrs_msgs/TransformVector3Srv.h>
+     105             : #include <mrs_msgs/TransformVector3SrvRequest.h>
+     106             : #include <mrs_msgs/TransformVector3SrvResponse.h>
+     107             : 
+     108             : #include <mrs_msgs/Float64StampedSrv.h>
+     109             : #include <mrs_msgs/Float64StampedSrvRequest.h>
+     110             : #include <mrs_msgs/Float64StampedSrvResponse.h>
+     111             : 
+     112             : #include <mrs_msgs/Vec4.h>
+     113             : #include <mrs_msgs/Vec4Request.h>
+     114             : #include <mrs_msgs/Vec4Response.h>
+     115             : 
+     116             : #include <mrs_msgs/Vec1.h>
+     117             : #include <mrs_msgs/Vec1Request.h>
+     118             : #include <mrs_msgs/Vec1Response.h>
+     119             : 
+     120             : //}
+     121             : 
+     122             : /* defines //{ */
+     123             : 
+     124             : #define TAU 2 * M_PI
+     125             : #define REF_X 0
+     126             : #define REF_Y 1
+     127             : #define REF_Z 2
+     128             : #define REF_HEADING 3
+     129             : #define ELAND_STR "eland"
+     130             : #define EHOVER_STR "ehover"
+     131             : #define ESCALATING_FAILSAFE_STR "escalating_failsafe"
+     132             : #define FAILSAFE_STR "failsafe"
+     133             : #define INPUT_UAV_STATE 0
+     134             : #define INPUT_ODOMETRY 1
+     135             : #define RC_DEADBAND 0.2
+     136             : 
+     137             : //}
+     138             : 
+     139             : /* using //{ */
+     140             : 
+     141             : using vec2_t = mrs_lib::geometry::vec_t<2>;
+     142             : using vec3_t = mrs_lib::geometry::vec_t<3>;
+     143             : 
+     144             : using radians  = mrs_lib::geometry::radians;
+     145             : using sradians = mrs_lib::geometry::sradians;
+     146             : 
+     147             : //}
+     148             : 
+     149             : namespace mrs_uav_managers
+     150             : {
+     151             : 
+     152             : namespace control_manager
+     153             : {
+     154             : 
+     155             : /* //{ class ControlManager */
+     156             : 
+     157             : // state machine
+     158             : typedef enum
+     159             : {
+     160             : 
+     161             :   IDLE_STATE,
+     162             :   LANDING_STATE,
+     163             : 
+     164             : } LandingStates_t;
+     165             : 
+     166             : const char* state_names[2] = {"IDLING", "LANDING"};
+     167             : 
+     168             : // state machine
+     169             : typedef enum
+     170             : {
+     171             : 
+     172             :   FCU_FRAME,
+     173             :   RELATIVE_FRAME,
+     174             :   ABSOLUTE_FRAME
+     175             : 
+     176             : } ReferenceFrameType_t;
+     177             : 
+     178             : // state machine
+     179             : typedef enum
+     180             : {
+     181             : 
+     182             :   ESC_NONE_STATE     = 0,
+     183             :   ESC_EHOVER_STATE   = 1,
+     184             :   ESC_ELAND_STATE    = 2,
+     185             :   ESC_FAILSAFE_STATE = 3,
+     186             :   ESC_FINISHED_STATE = 4,
+     187             : 
+     188             : } EscalatingFailsafeStates_t;
+     189             : 
+     190             : /* class ControllerParams() //{ */
+     191             : 
+     192             : class ControllerParams {
+     193             : 
+     194             : public:
+     195             :   ControllerParams(std::string address, std::string name_space, double eland_threshold, double failsafe_threshold, double odometry_innovation_threshold,
+     196             :                    bool human_switchable);
+     197             : 
+     198             : public:
+     199             :   double      failsafe_threshold;
+     200             :   double      eland_threshold;
+     201             :   double      odometry_innovation_threshold;
+     202             :   std::string address;
+     203             :   std::string name_space;
+     204             :   bool        human_switchable;
+     205             : };
+     206             : 
+     207         545 : ControllerParams::ControllerParams(std::string address, std::string name_space, double eland_threshold, double failsafe_threshold,
+     208         545 :                                    double odometry_innovation_threshold, bool human_switchable) {
+     209             : 
+     210         545 :   this->eland_threshold               = eland_threshold;
+     211         545 :   this->odometry_innovation_threshold = odometry_innovation_threshold;
+     212         545 :   this->failsafe_threshold            = failsafe_threshold;
+     213         545 :   this->address                       = address;
+     214         545 :   this->name_space                    = name_space;
+     215         545 :   this->human_switchable              = human_switchable;
+     216         545 : }
+     217             : 
+     218             : //}
+     219             : 
+     220             : /* class TrackerParams() //{ */
+     221             : 
+     222             : class TrackerParams {
+     223             : 
+     224             : public:
+     225             :   TrackerParams(std::string address, std::string name_space, bool human_switchable);
+     226             : 
+     227             : public:
+     228             :   std::string address;
+     229             :   std::string name_space;
+     230             :   bool        human_switchable;
+     231             : };
+     232             : 
+     233         655 : TrackerParams::TrackerParams(std::string address, std::string name_space, bool human_switchable) {
+     234             : 
+     235         655 :   this->address          = address;
+     236         655 :   this->name_space       = name_space;
+     237         655 :   this->human_switchable = human_switchable;
+     238         655 : }
+     239             : 
+     240             : //}
+     241             : 
+     242             : class ControlManager : public nodelet::Nodelet {
+     243             : 
+     244             : public:
+     245             :   virtual void onInit();
+     246             : 
+     247             : private:
+     248             :   ros::NodeHandle   nh_;
+     249             :   std::atomic<bool> is_initialized_ = false;
+     250             :   std::string       _uav_name_;
+     251             :   std::string       _body_frame_;
+     252             : 
+     253             :   std::string _custom_config_;
+     254             :   std::string _platform_config_;
+     255             :   std::string _world_config_;
+     256             :   std::string _network_config_;
+     257             : 
+     258             :   // | --------------- dynamic loading of trackers -------------- |
+     259             : 
+     260             :   std::unique_ptr<pluginlib::ClassLoader<mrs_uav_managers::Tracker>> tracker_loader_;  // pluginlib loader of dynamically loaded trackers
+     261             :   std::vector<std::string>                                           _tracker_names_;  // list of tracker names
+     262             :   std::map<std::string, TrackerParams>                               trackers_;        // map between tracker names and tracker param
+     263             :   std::vector<boost::shared_ptr<mrs_uav_managers::Tracker>>          tracker_list_;    // list of trackers, routines are callable from this
+     264             :   std::mutex                                                         mutex_tracker_list_;
+     265             : 
+     266             :   // | ------------- dynamic loading of controllers ------------- |
+     267             : 
+     268             :   std::unique_ptr<pluginlib::ClassLoader<mrs_uav_managers::Controller>> controller_loader_;  // pluginlib loader of dynamically loaded controllers
+     269             :   std::vector<std::string>                                              _controller_names_;  // list of controller names
+     270             :   std::map<std::string, ControllerParams>                               controllers_;        // map between controller names and controller params
+     271             :   std::vector<boost::shared_ptr<mrs_uav_managers::Controller>>          controller_list_;    // list of controllers, routines are callable from this
+     272             :   std::mutex                                                            mutex_controller_list_;
+     273             : 
+     274             :   // | ------------------------- HW API ------------------------- |
+     275             : 
+     276             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities> sh_hw_api_capabilities_;
+     277             : 
+     278             :   OutputPublisher control_output_publisher_;
+     279             : 
+     280             :   ControlOutputModalities_t _hw_api_inputs_;
+     281             : 
+     282             :   double desired_uav_state_rate_ = 100.0;
+     283             : 
+     284             :   // this timer will check till we already got the hardware api diagnostics
+     285             :   // then it will trigger the initialization of the controllers and finish
+     286             :   // the initialization of the ControlManager
+     287             :   ros::Timer timer_hw_api_capabilities_;
+     288             :   void       timerHwApiCapabilities(const ros::TimerEvent& event);
+     289             : 
+     290             :   void preinitialize(void);
+     291             :   void initialize(void);
+     292             : 
+     293             :   // | ------------ tracker and controller switching ------------ |
+     294             : 
+     295             :   std::tuple<bool, std::string> switchController(const std::string& controller_name);
+     296             :   std::tuple<bool, std::string> switchTracker(const std::string& tracker_name);
+     297             : 
+     298             :   // the time of last switching of a tracker or a controller
+     299             :   ros::Time  controller_tracker_switch_time_;
+     300             :   std::mutex mutex_controller_tracker_switch_time_;
+     301             : 
+     302             :   // | -------------------- the transformer  -------------------- |
+     303             : 
+     304             :   std::shared_ptr<mrs_lib::Transformer> transformer_;
+     305             : 
+     306             :   // | ------------------- scope timer logger ------------------- |
+     307             : 
+     308             :   bool                                       scope_timer_enabled_ = false;
+     309             :   std::shared_ptr<mrs_lib::ScopeTimerLogger> scope_timer_logger_;
+     310             : 
+     311             :   // | --------------------- general params --------------------- |
+     312             : 
+     313             :   // defines the type of state input: odometry or uav_state mesasge types
+     314             :   int _state_input_;
+     315             : 
+     316             :   // names of important trackers
+     317             :   std::string _null_tracker_name_;     // null tracker is active when UAV is not in the air
+     318             :   std::string _ehover_tracker_name_;   // ehover tracker is used for emergency hovering
+     319             :   std::string _landoff_tracker_name_;  // landoff is used for landing and takeoff
+     320             : 
+     321             :   // names of important controllers
+     322             :   std::string _failsafe_controller_name_;  // controller used for feed-forward failsafe
+     323             :   std::string _eland_controller_name_;     // controller used for emergancy landing
+     324             : 
+     325             :   // joystick control
+     326             :   bool        _joystick_enabled_ = false;
+     327             :   int         _joystick_mode_;
+     328             :   std::string _joystick_tracker_name_;
+     329             :   std::string _joystick_controller_name_;
+     330             :   std::string _joystick_fallback_tracker_name_;
+     331             :   std::string _joystick_fallback_controller_name_;
+     332             : 
+     333             :   // should disarm after emergancy landing?
+     334             :   bool _eland_disarm_enabled_ = false;
+     335             : 
+     336             :   // enabling the emergency handoff -> will disable eland and failsafe
+     337             :   bool _rc_emergency_handoff_ = false;
+     338             : 
+     339             :   // what throttle should be output when null tracker is active?
+     340             :   double _min_throttle_null_tracker_ = 0.0;
+     341             : 
+     342             :   // rates of all the timers
+     343             :   double _status_timer_rate_   = 0;
+     344             :   double _safety_timer_rate_   = 0;
+     345             :   double _elanding_timer_rate_ = 0;
+     346             :   double _failsafe_timer_rate_ = 0;
+     347             :   double _bumper_timer_rate_   = 0;
+     348             : 
+     349             :   bool _snap_trajectory_to_safety_area_ = false;
+     350             : 
+     351             :   // | -------------- uav_state/odometry subscriber ------------- |
+     352             : 
+     353             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_odometry_;
+     354             :   mrs_lib::SubscribeHandler<mrs_msgs::UavState> sh_uav_state_;
+     355             : 
+     356             :   mrs_msgs::UavState uav_state_;
+     357             :   mrs_msgs::UavState previous_uav_state_;
+     358             :   bool               got_uav_state_               = false;
+     359             :   double             _uav_state_max_missing_time_ = 0;  // how long should we tolerate missing state estimate?
+     360             :   double             uav_roll_                    = 0;
+     361             :   double             uav_pitch_                   = 0;
+     362             :   double             uav_yaw_                     = 0;
+     363             :   double             uav_heading_                 = 0;
+     364             :   std::mutex         mutex_uav_state_;
+     365             : 
+     366             :   // odometry hiccup detection
+     367             :   double uav_state_avg_dt_        = 1;
+     368             :   double uav_state_hiccup_factor_ = 1;
+     369             :   int    uav_state_count_         = 0;
+     370             : 
+     371             :   mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix> sh_gnss_;
+     372             : 
+     373             :   // | -------------- safety area max z subscriber -------------- |
+     374             : 
+     375             :   mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped> sh_max_z_;
+     376             : 
+     377             :   // | ------------- odometry innovation subscriber ------------- |
+     378             : 
+     379             :   // odometry innovation is published by the odometry node
+     380             :   // it is used to issue eland if the estimator's input is too wonky
+     381             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_odometry_innovation_;
+     382             : 
+     383             :   // | --------------------- common handlers -------------------- |
+     384             : 
+     385             :   // contains handlers that are shared with trackers and controllers
+     386             :   std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers_;
+     387             : 
+     388             :   // | --------------- tracker and controller IDs --------------- |
+     389             : 
+     390             :   // keeping track of currently active controllers and trackers
+     391             :   unsigned int active_tracker_idx_    = 0;
+     392             :   unsigned int active_controller_idx_ = 0;
+     393             : 
+     394             :   // indeces of some notable trackers
+     395             :   unsigned int _ehover_tracker_idx_               = 0;
+     396             :   unsigned int _landoff_tracker_idx_              = 0;
+     397             :   unsigned int _joystick_tracker_idx_             = 0;
+     398             :   unsigned int _joystick_controller_idx_          = 0;
+     399             :   unsigned int _failsafe_controller_idx_          = 0;
+     400             :   unsigned int _joystick_fallback_controller_idx_ = 0;
+     401             :   unsigned int _joystick_fallback_tracker_idx_    = 0;
+     402             :   unsigned int _null_tracker_idx_                 = 0;
+     403             :   unsigned int _eland_controller_idx_             = 0;
+     404             : 
+     405             :   // | -------------- enabling the output publisher ------------- |
+     406             : 
+     407             :   void              toggleOutput(const bool& input);
+     408             :   std::atomic<bool> output_enabled_ = false;
+     409             : 
+     410             :   // | ----------------------- publishers ----------------------- |
+     411             : 
+     412             :   mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics>     ph_controller_diagnostics_;
+     413             :   mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand>            ph_tracker_cmd_;
+     414             :   mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput>            ph_mrs_odom_input_;
+     415             :   mrs_lib::PublisherHandler<nav_msgs::Odometry>                  ph_control_reference_odom_;
+     416             :   mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics> ph_diagnostics_;
+     417             :   mrs_lib::PublisherHandler<std_msgs::Empty>                     ph_offboard_on_;
+     418             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>            ph_tilt_error_;
+     419             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_mass_estimate_;
+     420             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_mass_nominal_;
+     421             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_throttle_;
+     422             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_thrust_;
+     423             :   mrs_lib::PublisherHandler<mrs_msgs::ControlError>              ph_control_error_;
+     424             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>     ph_safety_area_markers_;
+     425             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>     ph_safety_area_coordinates_markers_;
+     426             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>     ph_disturbances_markers_;
+     427             :   mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints>       ph_current_constraints_;
+     428             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>            ph_heading_;
+     429             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>            ph_speed_;
+     430             : 
+     431             :   // | --------------------- service servers -------------------- |
+     432             : 
+     433             :   ros::ServiceServer service_server_switch_tracker_;
+     434             :   ros::ServiceServer service_server_switch_controller_;
+     435             :   ros::ServiceServer service_server_reset_tracker_;
+     436             :   ros::ServiceServer service_server_hover_;
+     437             :   ros::ServiceServer service_server_ehover_;
+     438             :   ros::ServiceServer service_server_failsafe_;
+     439             :   ros::ServiceServer service_server_failsafe_escalating_;
+     440             :   ros::ServiceServer service_server_toggle_output_;
+     441             :   ros::ServiceServer service_server_arm_;
+     442             :   ros::ServiceServer service_server_enable_callbacks_;
+     443             :   ros::ServiceServer service_server_set_constraints_;
+     444             :   ros::ServiceServer service_server_use_joystick_;
+     445             :   ros::ServiceServer service_server_use_safety_area_;
+     446             :   ros::ServiceServer service_server_emergency_reference_;
+     447             :   ros::ServiceServer service_server_pirouette_;
+     448             :   ros::ServiceServer service_server_eland_;
+     449             :   ros::ServiceServer service_server_parachute_;
+     450             :   ros::ServiceServer service_server_set_min_z_;
+     451             : 
+     452             :   // human callbable services for references
+     453             :   ros::ServiceServer service_server_goto_;
+     454             :   ros::ServiceServer service_server_goto_fcu_;
+     455             :   ros::ServiceServer service_server_goto_relative_;
+     456             :   ros::ServiceServer service_server_goto_altitude_;
+     457             :   ros::ServiceServer service_server_set_heading_;
+     458             :   ros::ServiceServer service_server_set_heading_relative_;
+     459             : 
+     460             :   // the reference service and subscriber
+     461             :   ros::ServiceServer                                    service_server_reference_;
+     462             :   mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped> sh_reference_;
+     463             : 
+     464             :   // the velocity reference service and subscriber
+     465             :   ros::ServiceServer                                            service_server_velocity_reference_;
+     466             :   mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped> sh_velocity_reference_;
+     467             : 
+     468             :   // trajectory tracking
+     469             :   ros::ServiceServer                                       service_server_trajectory_reference_;
+     470             :   mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference> sh_trajectory_reference_;
+     471             :   ros::ServiceServer                                       service_server_start_trajectory_tracking_;
+     472             :   ros::ServiceServer                                       service_server_stop_trajectory_tracking_;
+     473             :   ros::ServiceServer                                       service_server_resume_trajectory_tracking_;
+     474             :   ros::ServiceServer                                       service_server_goto_trajectory_start_;
+     475             : 
+     476             :   // transform service servers
+     477             :   ros::ServiceServer service_server_transform_reference_;
+     478             :   ros::ServiceServer service_server_transform_reference_array_;
+     479             :   ros::ServiceServer service_server_transform_pose_;
+     480             :   ros::ServiceServer service_server_transform_vector3_;
+     481             : 
+     482             :   // safety area services
+     483             :   ros::ServiceServer service_server_validate_reference_;
+     484             :   ros::ServiceServer service_server_validate_reference_2d_;
+     485             :   ros::ServiceServer service_server_validate_reference_array_;
+     486             : 
+     487             :   // bumper service servers
+     488             :   ros::ServiceServer service_server_bumper_enabler_;
+     489             :   ros::ServiceServer service_server_bumper_repulsion_enabler_;
+     490             : 
+     491             :   // service clients
+     492             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool> sch_arming_;
+     493             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_eland_;
+     494             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_shutdown_;
+     495             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool> sch_set_odometry_callbacks_;
+     496             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_parachute_;
+     497             : 
+     498             :   // safety area min z servers
+     499             :   ros::ServiceServer service_server_get_min_z_;
+     500             : 
+     501             :   // | --------- trackers' and controllers' last results -------- |
+     502             : 
+     503             :   // the last result of an active tracker
+     504             :   std::optional<mrs_msgs::TrackerCommand> last_tracker_cmd_;
+     505             :   std::mutex                              mutex_last_tracker_cmd_;
+     506             : 
+     507             :   // the last result of an active controller
+     508             :   Controller::ControlOutput last_control_output_;
+     509             :   std::mutex                mutex_last_control_output_;
+     510             : 
+     511             :   // | -------------- HW API diagnostics subscriber ------------- |
+     512             : 
+     513             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus> sh_hw_api_status_;
+     514             : 
+     515             :   bool offboard_mode_          = false;
+     516             :   bool offboard_mode_was_true_ = false;  // if it was ever true
+     517             :   bool armed_                  = false;
+     518             : 
+     519             :   // | -------------------- throttle and mass ------------------- |
+     520             : 
+     521             :   // throttle mass estimation during eland
+     522             :   double    throttle_mass_estimate_   = 0;
+     523             :   bool      throttle_under_threshold_ = false;
+     524             :   ros::Time throttle_mass_estimate_first_time_;
+     525             : 
+     526             :   // | ---------------------- safety params --------------------- |
+     527             : 
+     528             :   // failsafe when tilt error is too large
+     529             :   bool   _tilt_error_disarm_enabled_;
+     530             :   double _tilt_error_disarm_timeout_;
+     531             :   double _tilt_error_disarm_threshold_;
+     532             : 
+     533             :   ros::Time tilt_error_disarm_time_;
+     534             :   bool      tilt_error_disarm_over_thr_ = false;
+     535             : 
+     536             :   // elanding when tilt error is too large
+     537             :   bool   _tilt_limit_eland_enabled_;
+     538             :   double _tilt_limit_eland_ = 0;  // [rad]
+     539             : 
+     540             :   // disarming when tilt error is too large
+     541             :   bool   _tilt_limit_disarm_enabled_;
+     542             :   double _tilt_limit_disarm_ = 0;  // [rad]
+     543             : 
+     544             :   // elanding when yaw error is too large
+     545             :   bool   _yaw_error_eland_enabled_;
+     546             :   double _yaw_error_eland_ = 0;  // [rad]
+     547             : 
+     548             :   // keeping track of control errors
+     549             :   std::optional<double> tilt_error_ = 0;
+     550             :   std::optional<double> yaw_error_  = 0;
+     551             :   std::mutex            mutex_attitude_error_;
+     552             : 
+     553             :   std::optional<Eigen::Vector3d> position_error_;
+     554             :   std::mutex                     mutex_position_error_;
+     555             : 
+     556             :   // control error for triggering failsafe, eland, etc.
+     557             :   // this filled with the current controllers failsafe threshold
+     558             :   double _failsafe_threshold_                = 0;  // control error for triggering failsafe
+     559             :   double _eland_threshold_                   = 0;  // control error for triggering eland
+     560             :   bool   _odometry_innovation_check_enabled_ = false;
+     561             :   double _odometry_innovation_threshold_     = 0;  // innovation size for triggering eland
+     562             : 
+     563             :   bool callbacks_enabled_ = true;
+     564             : 
+     565             :   // | ------------------------ parachute ----------------------- |
+     566             : 
+     567             :   bool _parachute_enabled_ = false;
+     568             : 
+     569             :   std::tuple<bool, std::string> deployParachute(void);
+     570             :   bool                          parachuteSrv(void);
+     571             : 
+     572             :   // | ----------------------- safety area ---------------------- |
+     573             : 
+     574             :   // safety area
+     575             :   std::unique_ptr<mrs_lib::SafetyZone> safety_zone_;
+     576             : 
+     577             :   std::atomic<bool> use_safety_area_ = false;
+     578             : 
+     579             :   std::string _safety_area_horizontal_frame_;
+     580             :   std::string _safety_area_vertical_frame_;
+     581             : 
+     582             :   std::atomic<double> _safety_area_min_z_ = 0;
+     583             : 
+     584             :   double _safety_area_max_z_ = 0;
+     585             : 
+     586             :   // safety area routines
+     587             :   // those are passed to trackers using the common_handlers object
+     588             :   bool   isPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& point);
+     589             :   bool   isPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& point);
+     590             :   bool   isPathToPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& from, const mrs_msgs::ReferenceStamped& to);
+     591             :   bool   isPathToPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& from, const mrs_msgs::ReferenceStamped& to);
+     592             :   double getMinZ(const std::string& frame_id);
+     593             :   double getMaxZ(const std::string& frame_id);
+     594             : 
+     595             :   // | ------------------------ callbacks ----------------------- |
+     596             : 
+     597             :   // topic callbacks
+     598             :   void callbackOdometry(const nav_msgs::Odometry::ConstPtr msg);
+     599             :   void callbackUavState(const mrs_msgs::UavState::ConstPtr msg);
+     600             :   void callbackHwApiStatus(const mrs_msgs::HwApiStatus::ConstPtr msg);
+     601             :   void callbackGNSS(const sensor_msgs::NavSatFix::ConstPtr msg);
+     602             :   void callbackRC(const mrs_msgs::HwApiRcChannels::ConstPtr msg);
+     603             : 
+     604             :   // topic timeouts
+     605             :   void timeoutUavState(const double& missing_for);
+     606             : 
+     607             :   // switching controller and tracker services
+     608             :   bool callbackSwitchTracker(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+     609             :   bool callbackSwitchController(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+     610             :   bool callbackTrackerResetStatic(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     611             : 
+     612             :   // reference callbacks
+     613             :   void callbackReferenceTopic(const mrs_msgs::ReferenceStamped::ConstPtr msg);
+     614             :   void callbackVelocityReferenceTopic(const mrs_msgs::VelocityReferenceStamped::ConstPtr msg);
+     615             :   void callbackTrajectoryReferenceTopic(const mrs_msgs::TrajectoryReference::ConstPtr msg);
+     616             :   bool callbackGoto(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res);
+     617             :   bool callbackGotoFcu(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res);
+     618             :   bool callbackGotoRelative(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res);
+     619             :   bool callbackGotoAltitude(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+     620             :   bool callbackSetHeading(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+     621             :   bool callbackSetHeadingRelative(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+     622             :   bool callbackReferenceService(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res);
+     623             :   bool callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrv::Request& req, mrs_msgs::VelocityReferenceStampedSrv::Response& res);
+     624             :   bool callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrv::Request& req, mrs_msgs::TrajectoryReferenceSrv::Response& res);
+     625             :   bool callbackEmergencyReference(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res);
+     626             : 
+     627             :   // safety callbacks
+     628             :   bool callbackHover(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     629             :   bool callbackStartTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     630             :   bool callbackStopTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     631             :   bool callbackResumeTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     632             :   bool callbackGotoTrajectoryStart([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     633             :   bool callbackEHover(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     634             :   bool callbackFailsafe(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     635             :   bool callbackFailsafeEscalating(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     636             :   bool callbackEland(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     637             :   bool callbackParachute([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     638             :   bool callbackSetMinZ(mrs_msgs::Float64StampedSrv::Request& req, mrs_msgs::Float64StampedSrv::Response& res);
+     639             :   bool callbackToggleOutput(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     640             :   bool callbackArm(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     641             :   bool callbackEnableCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     642             :   bool callbackEnableBumper(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     643             :   bool callbackUseSafetyArea(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     644             : 
+     645             :   bool callbackGetMinZ(mrs_msgs::GetFloat64::Request& req, mrs_msgs::GetFloat64::Response& res);
+     646             : 
+     647             :   bool callbackValidateReference(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res);
+     648             :   bool callbackValidateReference2d(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res);
+     649             :   bool callbackValidateReferenceArray(mrs_msgs::ValidateReferenceArray::Request& req, mrs_msgs::ValidateReferenceArray::Response& res);
+     650             : 
+     651             :   // transformation callbacks
+     652             :   bool callbackTransformReference(mrs_msgs::TransformReferenceSrv::Request& req, mrs_msgs::TransformReferenceSrv::Response& res);
+     653             :   bool callbackTransformReferenceArray(mrs_msgs::TransformReferenceArraySrv::Request& req, mrs_msgs::TransformReferenceArraySrv::Response& res);
+     654             :   bool callbackTransformPose(mrs_msgs::TransformPoseSrv::Request& req, mrs_msgs::TransformPoseSrv::Response& res);
+     655             :   bool callbackTransformVector3(mrs_msgs::TransformVector3Srv::Request& req, mrs_msgs::TransformVector3Srv::Response& res);
+     656             : 
+     657             :   // | ----------------------- constraints ---------------------- |
+     658             : 
+     659             :   // sets constraints to all trackers
+     660             :   bool callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrv::Request& req, mrs_msgs::DynamicsConstraintsSrv::Response& res);
+     661             : 
+     662             :   // constraints management
+     663             :   bool              got_constraints_ = false;
+     664             :   std::mutex        mutex_constraints_;
+     665             :   void              setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     666             :   void              setConstraintsToTrackers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     667             :   void              setConstraintsToControllers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     668             :   std::atomic<bool> constraints_being_enforced_ = false;
+     669             : 
+     670             :   std::optional<mrs_msgs::DynamicsConstraintsSrvRequest> enforceControllersConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     671             : 
+     672             :   mrs_msgs::DynamicsConstraintsSrvRequest current_constraints_;
+     673             :   mrs_msgs::DynamicsConstraintsSrvRequest sanitized_constraints_;
+     674             : 
+     675             :   // | ------------------ emergency triggered? ------------------ |
+     676             : 
+     677             :   std::atomic<bool> failsafe_triggered_ = false;
+     678             :   std::atomic<bool> eland_triggered_    = false;
+     679             : 
+     680             :   // | ------------------------- timers ------------------------- |
+     681             : 
+     682             :   // timer for regular status publishing
+     683             :   ros::Timer timer_status_;
+     684             :   void       timerStatus(const ros::TimerEvent& event);
+     685             : 
+     686             :   // timer for issuing the failsafe landing
+     687             :   ros::Timer timer_failsafe_;
+     688             :   void       timerFailsafe(const ros::TimerEvent& event);
+     689             : 
+     690             :   // oneshot timer for running controllers and trackers
+     691             :   void              asyncControl(void);
+     692             :   std::atomic<bool> running_async_control_ = false;
+     693             :   std::future<void> async_control_result_;
+     694             : 
+     695             :   // timer for issuing emergancy landing
+     696             :   ros::Timer timer_eland_;
+     697             :   void       timerEland(const ros::TimerEvent& event);
+     698             : 
+     699             :   // timer for regular checking of controller errors
+     700             :   ros::Timer        timer_safety_;
+     701             :   void              timerSafety(const ros::TimerEvent& event);
+     702             :   std::atomic<bool> running_safety_timer_        = false;
+     703             :   std::atomic<bool> odometry_switch_in_progress_ = false;
+     704             : 
+     705             :   // timer for issuing the pirouette
+     706             :   ros::Timer timer_pirouette_;
+     707             :   void       timerPirouette(const ros::TimerEvent& event);
+     708             : 
+     709             :   // | --------------------- obstacle bumper -------------------- |
+     710             : 
+     711             :   // bumper timer
+     712             :   ros::Timer timer_bumper_;
+     713             :   void       timerBumper(const ros::TimerEvent& event);
+     714             : 
+     715             :   // bumper subscriber
+     716             :   mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors> sh_bumper_;
+     717             : 
+     718             :   bool        _bumper_switch_tracker_    = false;
+     719             :   bool        _bumper_switch_controller_ = false;
+     720             :   std::string _bumper_tracker_name_;
+     721             :   std::string _bumper_controller_name_;
+     722             :   std::string bumper_previous_tracker_;
+     723             :   std::string bumper_previous_controller_;
+     724             : 
+     725             :   std::atomic<bool> bumper_enabled_   = false;
+     726             :   std::atomic<bool> bumper_repulsing_ = false;
+     727             : 
+     728             :   bool _bumper_horizontal_derive_from_dynamics_;
+     729             :   bool _bumper_vertical_derive_from_dynamics_;
+     730             : 
+     731             :   double _bumper_horizontal_distance_ = 0;
+     732             :   double _bumper_vertical_distance_   = 0;
+     733             : 
+     734             :   double _bumper_horizontal_overshoot_ = 0;
+     735             :   double _bumper_vertical_overshoot_   = 0;
+     736             : 
+     737             :   int  bumperGetSectorId(const double& x, const double& y, const double& z);
+     738             :   void bumperPushFromObstacle(void);
+     739             : 
+     740             :   // | --------------- safety checks and failsafes -------------- |
+     741             : 
+     742             :   // escalating failsafe (eland -> failsafe -> disarm)
+     743             :   bool                       _service_escalating_failsafe_enabled_ = false;
+     744             :   bool                       _rc_escalating_failsafe_enabled_      = false;
+     745             :   double                     _escalating_failsafe_timeout_         = 0;
+     746             :   ros::Time                  escalating_failsafe_time_;
+     747             :   bool                       _escalating_failsafe_ehover_   = false;
+     748             :   bool                       _escalating_failsafe_eland_    = false;
+     749             :   bool                       _escalating_failsafe_failsafe_ = false;
+     750             :   double                     _rc_escalating_failsafe_threshold_;
+     751             :   int                        _rc_escalating_failsafe_channel_  = 0;
+     752             :   bool                       rc_escalating_failsafe_triggered_ = false;
+     753             :   EscalatingFailsafeStates_t state_escalating_failsafe_        = ESC_NONE_STATE;
+     754             : 
+     755             :   std::string _tracker_error_action_;
+     756             : 
+     757             :   // emergancy landing state machine
+     758             :   LandingStates_t current_state_landing_  = IDLE_STATE;
+     759             :   LandingStates_t previous_state_landing_ = IDLE_STATE;
+     760             :   std::mutex      mutex_landing_state_machine_;
+     761             :   void            changeLandingState(LandingStates_t new_state);
+     762             :   double          _uav_mass_ = 0;
+     763             :   double          _elanding_cutoff_mass_factor_;
+     764             :   double          _elanding_cutoff_timeout_;
+     765             :   double          landing_uav_mass_ = 0;
+     766             : 
+     767             :   // initial body disturbance loaded from params
+     768             :   double _initial_body_disturbance_x_ = 0;
+     769             :   double _initial_body_disturbance_y_ = 0;
+     770             : 
+     771             :   // profiling
+     772             :   mrs_lib::Profiler profiler_;
+     773             :   bool              _profiler_enabled_ = false;
+     774             : 
+     775             :   // diagnostics publishing
+     776             :   void       publishDiagnostics(void);
+     777             :   std::mutex mutex_diagnostics_;
+     778             : 
+     779             :   void                                             ungripSrv(void);
+     780             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_ungrip_;
+     781             : 
+     782             :   bool isFlyingNormally(void);
+     783             : 
+     784             :   // | ------------------------ pirouette ----------------------- |
+     785             : 
+     786             :   bool       _pirouette_enabled_ = false;
+     787             :   double     _pirouette_speed_;
+     788             :   double     _pirouette_timer_rate_;
+     789             :   std::mutex mutex_pirouette_;
+     790             :   double     pirouette_initial_heading_;
+     791             :   double     pirouette_iterator_;
+     792             :   bool       callbackPirouette(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     793             : 
+     794             :   // | -------------------- joystick control -------------------- |
+     795             : 
+     796             :   mrs_lib::SubscribeHandler<sensor_msgs::Joy> sh_joystick_;
+     797             : 
+     798             :   void callbackJoystick(const sensor_msgs::Joy::ConstPtr msg);
+     799             :   bool callbackUseJoystick([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     800             : 
+     801             :   // joystick buttons mappings
+     802             :   int _channel_A_, _channel_B_, _channel_X_, _channel_Y_, _channel_start_, _channel_back_, _channel_LT_, _channel_RT_, _channel_L_joy_, _channel_R_joy_;
+     803             : 
+     804             :   // channel numbers and channel multipliers
+     805             :   int    _channel_pitch_, _channel_roll_, _channel_heading_, _channel_throttle_;
+     806             :   double _channel_mult_pitch_, _channel_mult_roll_, _channel_mult_heading_, _channel_mult_throttle_;
+     807             : 
+     808             :   ros::Timer timer_joystick_;
+     809             :   void       timerJoystick(const ros::TimerEvent& event);
+     810             :   double     _joystick_timer_rate_ = 0;
+     811             : 
+     812             :   double _joystick_carrot_distance_ = 0;
+     813             : 
+     814             :   ros::Time joystick_start_press_time_;
+     815             :   bool      joystick_start_pressed_ = false;
+     816             : 
+     817             :   ros::Time joystick_back_press_time_;
+     818             :   bool      joystick_back_pressed_ = false;
+     819             :   bool      joystick_goto_enabled_ = false;
+     820             : 
+     821             :   bool      joystick_failsafe_pressed_ = false;
+     822             :   ros::Time joystick_failsafe_press_time_;
+     823             : 
+     824             :   bool      joystick_eland_pressed_ = false;
+     825             :   ros::Time joystick_eland_press_time_;
+     826             : 
+     827             :   // | ------------------- RC joystick control ------------------ |
+     828             : 
+     829             :   // listening to the RC channels as told by pixhawk
+     830             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels> sh_hw_api_rc_;
+     831             : 
+     832             :   // the RC channel mapping of the main 4 control signals
+     833             :   double _rc_channel_pitch_, _rc_channel_roll_, _rc_channel_heading_, _rc_channel_throttle_;
+     834             : 
+     835             :   bool              _rc_goto_enabled_               = false;
+     836             :   std::atomic<bool> rc_goto_active_                 = false;
+     837             :   double            rc_joystick_channel_last_value_ = 0.5;
+     838             :   bool              rc_joystick_channel_was_low_    = false;
+     839             :   int               _rc_joystick_channel_           = 0;
+     840             : 
+     841             :   double _rc_horizontal_speed_ = 0;
+     842             :   double _rc_vertical_speed_   = 0;
+     843             :   double _rc_heading_rate_     = 0;
+     844             : 
+     845             :   // | ------------------- trajectory loading ------------------- |
+     846             : 
+     847             :   mrs_lib::PublisherHandler<geometry_msgs::PoseArray>        pub_debug_original_trajectory_poses_;
+     848             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray> pub_debug_original_trajectory_markers_;
+     849             : 
+     850             :   // | --------------------- other routines --------------------- |
+     851             : 
+     852             :   // this is called to update the trackers and to receive position control command from the active one
+     853             :   void updateTrackers(void);
+     854             : 
+     855             :   // this is called to update the controllers and to receive attitude control command from the active one
+     856             :   void updateControllers(const mrs_msgs::UavState& uav_state);
+     857             : 
+     858             :   // sets the reference to the active tracker
+     859             :   std::tuple<bool, std::string> setReference(const mrs_msgs::ReferenceStamped reference_in);
+     860             : 
+     861             :   // sets the velocity reference to the active tracker
+     862             :   std::tuple<bool, std::string> setVelocityReference(const mrs_msgs::VelocityReferenceStamped& reference_in);
+     863             : 
+     864             :   // sets the reference trajectory to the active tracker
+     865             :   std::tuple<bool, std::string, bool, std::vector<std::string>, std::vector<bool>, std::vector<std::string>> setTrajectoryReference(
+     866             :       const mrs_msgs::TrajectoryReference trajectory_in);
+     867             : 
+     868             :   // publishes
+     869             :   void publish(void);
+     870             : 
+     871             :   bool loadConfigFile(const std::string& file_path, const std::string ns);
+     872             : 
+     873             :   double getMass(void);
+     874             : 
+     875             :   // publishes rviz-visualizable control reference
+     876             :   void publishControlReferenceOdom(const std::optional<mrs_msgs::TrackerCommand>& tracker_command, const Controller::ControlOutput& control_output);
+     877             : 
+     878             :   void initializeControlOutput(void);
+     879             : 
+     880             :   // tell the mrs_odometry to disable its callbacks
+     881             :   void odometryCallbacksSrv(const bool input);
+     882             : 
+     883             :   mrs_msgs::ReferenceStamped velocityReferenceToReference(const mrs_msgs::VelocityReferenceStamped& vel_reference);
+     884             : 
+     885             :   void                          setCallbacks(bool in);
+     886             :   bool                          isOffboard(void);
+     887             :   bool                          elandSrv(void);
+     888             :   std::tuple<bool, std::string> arming(const bool input);
+     889             : 
+     890             :   // safety functions impl
+     891             :   std::tuple<bool, std::string> ehover(void);
+     892             :   std::tuple<bool, std::string> hover(void);
+     893             :   std::tuple<bool, std::string> startTrajectoryTracking(void);
+     894             :   std::tuple<bool, std::string> stopTrajectoryTracking(void);
+     895             :   std::tuple<bool, std::string> resumeTrajectoryTracking(void);
+     896             :   std::tuple<bool, std::string> gotoTrajectoryStart(void);
+     897             :   std::tuple<bool, std::string> eland(void);
+     898             :   std::tuple<bool, std::string> failsafe(void);
+     899             :   std::tuple<bool, std::string> escalatingFailsafe(void);
+     900             : 
+     901             :   EscalatingFailsafeStates_t getNextEscFailsafeState(void);
+     902             : };
+     903             : 
+     904             : //}
+     905             : 
+     906             : /* //{ onInit() */
+     907             : 
+     908         109 : void ControlManager::onInit() {
+     909         109 :   preinitialize();
+     910         109 : }
+     911             : 
+     912             : //}
+     913             : 
+     914             : /* preinitialize() //{ */
+     915             : 
+     916         109 : void ControlManager::preinitialize(void) {
+     917             : 
+     918         109 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     919             : 
+     920         109 :   ros::Time::waitForValid();
+     921             : 
+     922         109 :   mrs_lib::SubscribeHandlerOptions shopts;
+     923         109 :   shopts.nh                 = nh_;
+     924         109 :   shopts.node_name          = "ControlManager";
+     925         109 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     926         109 :   shopts.threadsafe         = true;
+     927         109 :   shopts.autostart          = true;
+     928         109 :   shopts.queue_size         = 10;
+     929         109 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     930             : 
+     931         109 :   sh_hw_api_capabilities_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities>(shopts, "hw_api_capabilities_in");
+     932             : 
+     933         109 :   timer_hw_api_capabilities_ = nh_.createTimer(ros::Rate(1.0), &ControlManager::timerHwApiCapabilities, this);
+     934         109 : }
+     935             : 
+     936             : //}
+     937             : 
+     938             : /* initialize() //{ */
+     939             : 
+     940         109 : void ControlManager::initialize(void) {
+     941             : 
+     942         109 :   joystick_start_press_time_      = ros::Time(0);
+     943         109 :   joystick_failsafe_press_time_   = ros::Time(0);
+     944         109 :   joystick_eland_press_time_      = ros::Time(0);
+     945         109 :   escalating_failsafe_time_       = ros::Time(0);
+     946         109 :   controller_tracker_switch_time_ = ros::Time(0);
+     947             : 
+     948         109 :   ROS_INFO("[ControlManager]: initializing");
+     949             : 
+     950             :   // --------------------------------------------------------------
+     951             :   // |         common handler for trackers and controllers        |
+     952             :   // --------------------------------------------------------------
+     953             : 
+     954         109 :   common_handlers_ = std::make_shared<mrs_uav_managers::control_manager::CommonHandlers_t>();
+     955             : 
+     956             :   // --------------------------------------------------------------
+     957             :   // |                           params                           |
+     958             :   // --------------------------------------------------------------
+     959             : 
+     960         218 :   mrs_lib::ParamLoader param_loader(nh_, "ControlManager");
+     961             : 
+     962         109 :   param_loader.loadParam("custom_config", _custom_config_);
+     963         109 :   param_loader.loadParam("platform_config", _platform_config_);
+     964         109 :   param_loader.loadParam("world_config", _world_config_);
+     965         109 :   param_loader.loadParam("network_config", _network_config_);
+     966             : 
+     967         109 :   if (_custom_config_ != "") {
+     968         109 :     param_loader.addYamlFile(_custom_config_);
+     969             :   }
+     970             : 
+     971         109 :   if (_platform_config_ != "") {
+     972         109 :     param_loader.addYamlFile(_platform_config_);
+     973             :   }
+     974             : 
+     975         109 :   if (_world_config_ != "") {
+     976         109 :     param_loader.addYamlFile(_world_config_);
+     977             :   }
+     978             : 
+     979         109 :   if (_network_config_ != "") {
+     980         109 :     param_loader.addYamlFile(_network_config_);
+     981             :   }
+     982             : 
+     983         109 :   param_loader.addYamlFileFromParam("private_config");
+     984         109 :   param_loader.addYamlFileFromParam("public_config");
+     985         109 :   param_loader.addYamlFileFromParam("private_trackers");
+     986         109 :   param_loader.addYamlFileFromParam("private_controllers");
+     987         109 :   param_loader.addYamlFileFromParam("public_controllers");
+     988             : 
+     989             :   // params passed from the launch file are not prefixed
+     990         109 :   param_loader.loadParam("uav_name", _uav_name_);
+     991         109 :   param_loader.loadParam("body_frame", _body_frame_);
+     992         109 :   param_loader.loadParam("enable_profiler", _profiler_enabled_);
+     993         109 :   param_loader.loadParam("uav_mass", _uav_mass_);
+     994         109 :   param_loader.loadParam("body_disturbance_x", _initial_body_disturbance_x_);
+     995         109 :   param_loader.loadParam("body_disturbance_y", _initial_body_disturbance_y_);
+     996         109 :   param_loader.loadParam("g", common_handlers_->g);
+     997             : 
+     998             :   // motor params are also not prefixed, since they are common to more nodes
+     999         109 :   param_loader.loadParam("motor_params/a", common_handlers_->throttle_model.A);
+    1000         109 :   param_loader.loadParam("motor_params/b", common_handlers_->throttle_model.B);
+    1001         109 :   param_loader.loadParam("motor_params/n_motors", common_handlers_->throttle_model.n_motors);
+    1002             : 
+    1003             :   // | ----------------------- safety area ---------------------- |
+    1004             : 
+    1005             :   bool use_safety_area;
+    1006         109 :   param_loader.loadParam("safety_area/enabled", use_safety_area);
+    1007         109 :   use_safety_area_ = use_safety_area;
+    1008             : 
+    1009         109 :   param_loader.loadParam("safety_area/horizontal/frame_name", _safety_area_horizontal_frame_);
+    1010             : 
+    1011         109 :   param_loader.loadParam("safety_area/vertical/frame_name", _safety_area_vertical_frame_);
+    1012         109 :   param_loader.loadParam("safety_area/vertical/max_z", _safety_area_max_z_);
+    1013             : 
+    1014             :   {
+    1015             :     double temp;
+    1016         109 :     param_loader.loadParam("safety_area/vertical/min_z", temp);
+    1017             : 
+    1018         109 :     _safety_area_min_z_ = temp;
+    1019             :   }
+    1020             : 
+    1021         109 :   if (use_safety_area_) {
+    1022             : 
+    1023         261 :     Eigen::MatrixXd border_points = param_loader.loadMatrixDynamic2("safety_area/horizontal/points", -1, 2);
+    1024             : 
+    1025             :     try {
+    1026             : 
+    1027         174 :       std::vector<Eigen::MatrixXd> polygon_obstacle_points;
+    1028          87 :       std::vector<Eigen::MatrixXd> point_obstacle_points;
+    1029             : 
+    1030          87 :       safety_zone_ = std::make_unique<mrs_lib::SafetyZone>(border_points);
+    1031             :     }
+    1032             : 
+    1033           0 :     catch (mrs_lib::SafetyZone::BorderError& e) {
+    1034           0 :       ROS_ERROR("[ControlManager]: SafetyArea: wrong configruation for the safety zone border polygon");
+    1035           0 :       ros::shutdown();
+    1036             :     }
+    1037           0 :     catch (...) {
+    1038           0 :       ROS_ERROR("[ControlManager]: SafetyArea: unhandled exception!");
+    1039           0 :       ros::shutdown();
+    1040             :     }
+    1041             : 
+    1042          87 :     ROS_INFO("[ControlManager]: safety area initialized");
+    1043             :   }
+    1044             : 
+    1045         109 :   param_loader.setPrefix("mrs_uav_managers/control_manager/");
+    1046             : 
+    1047         109 :   param_loader.loadParam("state_input", _state_input_);
+    1048             : 
+    1049         109 :   if (!(_state_input_ == INPUT_UAV_STATE || _state_input_ == INPUT_ODOMETRY)) {
+    1050           0 :     ROS_ERROR("[ControlManager]: the state_input parameter has to be in {0, 1}");
+    1051           0 :     ros::shutdown();
+    1052             :   }
+    1053             : 
+    1054         109 :   param_loader.loadParam("safety/min_throttle_null_tracker", _min_throttle_null_tracker_);
+    1055         109 :   param_loader.loadParam("safety/ehover_tracker", _ehover_tracker_name_);
+    1056         109 :   param_loader.loadParam("safety/failsafe_controller", _failsafe_controller_name_);
+    1057             : 
+    1058         109 :   param_loader.loadParam("safety/eland/controller", _eland_controller_name_);
+    1059         109 :   param_loader.loadParam("safety/eland/cutoff_mass_factor", _elanding_cutoff_mass_factor_);
+    1060         109 :   param_loader.loadParam("safety/eland/cutoff_timeout", _elanding_cutoff_timeout_);
+    1061         109 :   param_loader.loadParam("safety/eland/timer_rate", _elanding_timer_rate_);
+    1062         109 :   param_loader.loadParam("safety/eland/disarm", _eland_disarm_enabled_);
+    1063             : 
+    1064         109 :   param_loader.loadParam("safety/escalating_failsafe/service/enabled", _service_escalating_failsafe_enabled_);
+    1065         109 :   param_loader.loadParam("safety/escalating_failsafe/rc/enabled", _rc_escalating_failsafe_enabled_);
+    1066         109 :   param_loader.loadParam("safety/escalating_failsafe/rc/channel_number", _rc_escalating_failsafe_channel_);
+    1067         109 :   param_loader.loadParam("safety/escalating_failsafe/rc/threshold", _rc_escalating_failsafe_threshold_);
+    1068         109 :   param_loader.loadParam("safety/escalating_failsafe/timeout", _escalating_failsafe_timeout_);
+    1069         109 :   param_loader.loadParam("safety/escalating_failsafe/ehover", _escalating_failsafe_ehover_);
+    1070         109 :   param_loader.loadParam("safety/escalating_failsafe/eland", _escalating_failsafe_eland_);
+    1071         109 :   param_loader.loadParam("safety/escalating_failsafe/failsafe", _escalating_failsafe_failsafe_);
+    1072             : 
+    1073         109 :   param_loader.loadParam("safety/tilt_limit/eland/enabled", _tilt_limit_eland_enabled_);
+    1074         109 :   param_loader.loadParam("safety/tilt_limit/eland/limit", _tilt_limit_eland_);
+    1075             : 
+    1076         109 :   _tilt_limit_eland_ = M_PI * (_tilt_limit_eland_ / 180.0);
+    1077             : 
+    1078         109 :   if (_tilt_limit_eland_enabled_ && fabs(_tilt_limit_eland_) < 1e-3) {
+    1079           0 :     ROS_ERROR("[ControlManager]: safety/tilt_limit/eland/enabled = 'TRUE' but the limit is too low");
+    1080           0 :     ros::shutdown();
+    1081             :   }
+    1082             : 
+    1083         109 :   param_loader.loadParam("safety/tilt_limit/disarm/enabled", _tilt_limit_disarm_enabled_);
+    1084         109 :   param_loader.loadParam("safety/tilt_limit/disarm/limit", _tilt_limit_disarm_);
+    1085             : 
+    1086         109 :   _tilt_limit_disarm_ = M_PI * (_tilt_limit_disarm_ / 180.0);
+    1087             : 
+    1088         109 :   if (_tilt_limit_disarm_enabled_ && fabs(_tilt_limit_disarm_) < 1e-3) {
+    1089           0 :     ROS_ERROR("[ControlManager]: safety/tilt_limit/disarm/enabled = 'TRUE' but the limit is too low");
+    1090           0 :     ros::shutdown();
+    1091             :   }
+    1092             : 
+    1093         109 :   param_loader.loadParam("safety/yaw_error_eland/enabled", _yaw_error_eland_enabled_);
+    1094         109 :   param_loader.loadParam("safety/yaw_error_eland/limit", _yaw_error_eland_);
+    1095             : 
+    1096         109 :   _yaw_error_eland_ = M_PI * (_yaw_error_eland_ / 180.0);
+    1097             : 
+    1098         109 :   if (_yaw_error_eland_enabled_ && fabs(_yaw_error_eland_) < 1e-3) {
+    1099           0 :     ROS_ERROR("[ControlManager]: safety/yaw_error_eland/enabled = 'TRUE' but the limit is too low");
+    1100           0 :     ros::shutdown();
+    1101             :   }
+    1102             : 
+    1103         109 :   param_loader.loadParam("status_timer_rate", _status_timer_rate_);
+    1104         109 :   param_loader.loadParam("safety/safety_timer_rate", _safety_timer_rate_);
+    1105         109 :   param_loader.loadParam("safety/failsafe_timer_rate", _failsafe_timer_rate_);
+    1106         109 :   param_loader.loadParam("safety/rc_emergency_handoff/enabled", _rc_emergency_handoff_);
+    1107             : 
+    1108         109 :   param_loader.loadParam("safety/odometry_max_missing_time", _uav_state_max_missing_time_);
+    1109         109 :   param_loader.loadParam("safety/odometry_innovation_eland/enabled", _odometry_innovation_check_enabled_);
+    1110             : 
+    1111         109 :   param_loader.loadParam("safety/tilt_error_disarm/enabled", _tilt_error_disarm_enabled_);
+    1112         109 :   param_loader.loadParam("safety/tilt_error_disarm/timeout", _tilt_error_disarm_timeout_);
+    1113         109 :   param_loader.loadParam("safety/tilt_error_disarm/error_threshold", _tilt_error_disarm_threshold_);
+    1114             : 
+    1115         109 :   _tilt_error_disarm_threshold_ = M_PI * (_tilt_error_disarm_threshold_ / 180.0);
+    1116             : 
+    1117         109 :   if (_tilt_error_disarm_enabled_ && fabs(_tilt_error_disarm_threshold_) < 1e-3) {
+    1118           0 :     ROS_ERROR("[ControlManager]: safety/tilt_error_disarm/enabled = 'TRUE' but the limit is too low");
+    1119           0 :     ros::shutdown();
+    1120             :   }
+    1121             : 
+    1122             :   // default constraints
+    1123             : 
+    1124         109 :   param_loader.loadParam("default_constraints/horizontal/speed", current_constraints_.constraints.horizontal_speed);
+    1125         109 :   param_loader.loadParam("default_constraints/horizontal/acceleration", current_constraints_.constraints.horizontal_acceleration);
+    1126         109 :   param_loader.loadParam("default_constraints/horizontal/jerk", current_constraints_.constraints.horizontal_jerk);
+    1127         109 :   param_loader.loadParam("default_constraints/horizontal/snap", current_constraints_.constraints.horizontal_snap);
+    1128             : 
+    1129         109 :   param_loader.loadParam("default_constraints/vertical/ascending/speed", current_constraints_.constraints.vertical_ascending_speed);
+    1130         109 :   param_loader.loadParam("default_constraints/vertical/ascending/acceleration", current_constraints_.constraints.vertical_ascending_acceleration);
+    1131         109 :   param_loader.loadParam("default_constraints/vertical/ascending/jerk", current_constraints_.constraints.vertical_ascending_jerk);
+    1132         109 :   param_loader.loadParam("default_constraints/vertical/ascending/snap", current_constraints_.constraints.vertical_ascending_snap);
+    1133             : 
+    1134         109 :   param_loader.loadParam("default_constraints/vertical/descending/speed", current_constraints_.constraints.vertical_descending_speed);
+    1135         109 :   param_loader.loadParam("default_constraints/vertical/descending/acceleration", current_constraints_.constraints.vertical_descending_acceleration);
+    1136         109 :   param_loader.loadParam("default_constraints/vertical/descending/jerk", current_constraints_.constraints.vertical_descending_jerk);
+    1137         109 :   param_loader.loadParam("default_constraints/vertical/descending/snap", current_constraints_.constraints.vertical_descending_snap);
+    1138             : 
+    1139         109 :   param_loader.loadParam("default_constraints/heading/speed", current_constraints_.constraints.heading_speed);
+    1140         109 :   param_loader.loadParam("default_constraints/heading/acceleration", current_constraints_.constraints.heading_acceleration);
+    1141         109 :   param_loader.loadParam("default_constraints/heading/jerk", current_constraints_.constraints.heading_jerk);
+    1142         109 :   param_loader.loadParam("default_constraints/heading/snap", current_constraints_.constraints.heading_snap);
+    1143             : 
+    1144         109 :   param_loader.loadParam("default_constraints/angular_speed/roll", current_constraints_.constraints.roll_rate);
+    1145         109 :   param_loader.loadParam("default_constraints/angular_speed/pitch", current_constraints_.constraints.pitch_rate);
+    1146         109 :   param_loader.loadParam("default_constraints/angular_speed/yaw", current_constraints_.constraints.yaw_rate);
+    1147             : 
+    1148         109 :   param_loader.loadParam("default_constraints/tilt", current_constraints_.constraints.tilt);
+    1149             : 
+    1150         109 :   current_constraints_.constraints.tilt = M_PI * (current_constraints_.constraints.tilt / 180.0);
+    1151             : 
+    1152             :   // joystick
+    1153             : 
+    1154         109 :   param_loader.loadParam("joystick/enabled", _joystick_enabled_);
+    1155         109 :   param_loader.loadParam("joystick/mode", _joystick_mode_);
+    1156         109 :   param_loader.loadParam("joystick/carrot_distance", _joystick_carrot_distance_);
+    1157         109 :   param_loader.loadParam("joystick/joystick_timer_rate", _joystick_timer_rate_);
+    1158         109 :   param_loader.loadParam("joystick/attitude_control/tracker", _joystick_tracker_name_);
+    1159         109 :   param_loader.loadParam("joystick/attitude_control/controller", _joystick_controller_name_);
+    1160         109 :   param_loader.loadParam("joystick/attitude_control/fallback/tracker", _joystick_fallback_tracker_name_);
+    1161         109 :   param_loader.loadParam("joystick/attitude_control/fallback/controller", _joystick_fallback_controller_name_);
+    1162             : 
+    1163         109 :   param_loader.loadParam("joystick/channels/A", _channel_A_);
+    1164         109 :   param_loader.loadParam("joystick/channels/B", _channel_B_);
+    1165         109 :   param_loader.loadParam("joystick/channels/X", _channel_X_);
+    1166         109 :   param_loader.loadParam("joystick/channels/Y", _channel_Y_);
+    1167         109 :   param_loader.loadParam("joystick/channels/start", _channel_start_);
+    1168         109 :   param_loader.loadParam("joystick/channels/back", _channel_back_);
+    1169         109 :   param_loader.loadParam("joystick/channels/LT", _channel_LT_);
+    1170         109 :   param_loader.loadParam("joystick/channels/RT", _channel_RT_);
+    1171         109 :   param_loader.loadParam("joystick/channels/L_joy", _channel_L_joy_);
+    1172         109 :   param_loader.loadParam("joystick/channels/R_joy", _channel_R_joy_);
+    1173             : 
+    1174             :   // load channels
+    1175         109 :   param_loader.loadParam("joystick/channels/pitch", _channel_pitch_);
+    1176         109 :   param_loader.loadParam("joystick/channels/roll", _channel_roll_);
+    1177         109 :   param_loader.loadParam("joystick/channels/heading", _channel_heading_);
+    1178         109 :   param_loader.loadParam("joystick/channels/throttle", _channel_throttle_);
+    1179             : 
+    1180             :   // load channel multipliers
+    1181         109 :   param_loader.loadParam("joystick/channel_multipliers/pitch", _channel_mult_pitch_);
+    1182         109 :   param_loader.loadParam("joystick/channel_multipliers/roll", _channel_mult_roll_);
+    1183         109 :   param_loader.loadParam("joystick/channel_multipliers/heading", _channel_mult_heading_);
+    1184         109 :   param_loader.loadParam("joystick/channel_multipliers/throttle", _channel_mult_throttle_);
+    1185             : 
+    1186             :   bool bumper_enabled;
+    1187         109 :   param_loader.loadParam("obstacle_bumper/enabled", bumper_enabled);
+    1188         109 :   bumper_enabled_ = bumper_enabled;
+    1189             : 
+    1190         109 :   param_loader.loadParam("obstacle_bumper/switch_tracker", _bumper_switch_tracker_);
+    1191         109 :   param_loader.loadParam("obstacle_bumper/switch_controller", _bumper_switch_controller_);
+    1192         109 :   param_loader.loadParam("obstacle_bumper/tracker", _bumper_tracker_name_);
+    1193         109 :   param_loader.loadParam("obstacle_bumper/controller", _bumper_controller_name_);
+    1194         109 :   param_loader.loadParam("obstacle_bumper/timer_rate", _bumper_timer_rate_);
+    1195             : 
+    1196         109 :   param_loader.loadParam("obstacle_bumper/horizontal/min_distance_to_obstacle", _bumper_horizontal_distance_);
+    1197         109 :   param_loader.loadParam("obstacle_bumper/horizontal/derived_from_dynamics", _bumper_horizontal_derive_from_dynamics_);
+    1198             : 
+    1199         109 :   param_loader.loadParam("obstacle_bumper/vertical/min_distance_to_obstacle", _bumper_vertical_distance_);
+    1200         109 :   param_loader.loadParam("obstacle_bumper/vertical/derived_from_dynamics", _bumper_vertical_derive_from_dynamics_);
+    1201             : 
+    1202         109 :   param_loader.loadParam("obstacle_bumper/horizontal/overshoot", _bumper_horizontal_overshoot_);
+    1203         109 :   param_loader.loadParam("obstacle_bumper/vertical/overshoot", _bumper_vertical_overshoot_);
+    1204             : 
+    1205         109 :   param_loader.loadParam("safety/tracker_error_action", _tracker_error_action_);
+    1206             : 
+    1207         109 :   param_loader.loadParam("trajectory_tracking/snap_to_safety_area", _snap_trajectory_to_safety_area_);
+    1208             : 
+    1209             :   // check the values of tracker error action
+    1210         109 :   if (_tracker_error_action_ != ELAND_STR && _tracker_error_action_ != EHOVER_STR) {
+    1211           0 :     ROS_ERROR("[ControlManager]: the tracker_error_action parameter (%s) is not correct, requires {%s, %s}", _tracker_error_action_.c_str(), ELAND_STR,
+    1212             :               EHOVER_STR);
+    1213           0 :     ros::shutdown();
+    1214             :   }
+    1215             : 
+    1216         109 :   param_loader.loadParam("rc_joystick/enabled", _rc_goto_enabled_);
+    1217         109 :   param_loader.loadParam("rc_joystick/channel_number", _rc_joystick_channel_);
+    1218         109 :   param_loader.loadParam("rc_joystick/horizontal_speed", _rc_horizontal_speed_);
+    1219         109 :   param_loader.loadParam("rc_joystick/vertical_speed", _rc_vertical_speed_);
+    1220         109 :   param_loader.loadParam("rc_joystick/heading_rate", _rc_heading_rate_);
+    1221             : 
+    1222         109 :   param_loader.loadParam("rc_joystick/channels/pitch", _rc_channel_pitch_);
+    1223         109 :   param_loader.loadParam("rc_joystick/channels/roll", _rc_channel_roll_);
+    1224         109 :   param_loader.loadParam("rc_joystick/channels/heading", _rc_channel_heading_);
+    1225         109 :   param_loader.loadParam("rc_joystick/channels/throttle", _rc_channel_throttle_);
+    1226             : 
+    1227         109 :   param_loader.loadParam("pirouette/speed", _pirouette_speed_);
+    1228         109 :   param_loader.loadParam("pirouette/timer_rate", _pirouette_timer_rate_);
+    1229             : 
+    1230         109 :   param_loader.loadParam("safety/parachute/enabled", _parachute_enabled_);
+    1231             : 
+    1232             :   // --------------------------------------------------------------
+    1233             :   // |             initialize the last control output             |
+    1234             :   // --------------------------------------------------------------
+    1235             : 
+    1236         109 :   initializeControlOutput();
+    1237             : 
+    1238             :   // | --------------------- tf transformer --------------------- |
+    1239             : 
+    1240         109 :   transformer_ = std::make_shared<mrs_lib::Transformer>(nh_, "ControlManager");
+    1241         109 :   transformer_->setDefaultPrefix(_uav_name_);
+    1242         109 :   transformer_->retryLookupNewest(true);
+    1243             : 
+    1244             :   // | ------------------- scope timer logger ------------------- |
+    1245             : 
+    1246         109 :   param_loader.loadParam("scope_timer/enabled", scope_timer_enabled_);
+    1247         327 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+    1248         109 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+    1249             : 
+    1250             :   // bind transformer to trackers and controllers for use
+    1251         109 :   common_handlers_->transformer = transformer_;
+    1252             : 
+    1253             :   // bind scope timer to trackers and controllers for use
+    1254         109 :   common_handlers_->scope_timer.enabled = scope_timer_enabled_;
+    1255         109 :   common_handlers_->scope_timer.logger  = scope_timer_logger_;
+    1256             : 
+    1257         109 :   common_handlers_->safety_area.use_safety_area       = use_safety_area_;
+    1258         109 :   common_handlers_->safety_area.isPointInSafetyArea2d = boost::bind(&ControlManager::isPointInSafetyArea2d, this, _1);
+    1259         109 :   common_handlers_->safety_area.isPointInSafetyArea3d = boost::bind(&ControlManager::isPointInSafetyArea3d, this, _1);
+    1260         109 :   common_handlers_->safety_area.getMinZ               = boost::bind(&ControlManager::getMinZ, this, _1);
+    1261         109 :   common_handlers_->safety_area.getMaxZ               = boost::bind(&ControlManager::getMaxZ, this, _1);
+    1262             : 
+    1263         109 :   common_handlers_->getMass = boost::bind(&ControlManager::getMass, this);
+    1264             : 
+    1265         109 :   common_handlers_->detailed_model_params = loadDetailedUavModelParams(nh_, "ControlManager", _platform_config_, _custom_config_);
+    1266             : 
+    1267         109 :   common_handlers_->control_output_modalities = _hw_api_inputs_;
+    1268             : 
+    1269         109 :   common_handlers_->uav_name = _uav_name_;
+    1270             : 
+    1271         109 :   common_handlers_->parent_nh = nh_;
+    1272             : 
+    1273             :   // --------------------------------------------------------------
+    1274             :   // |                        load trackers                       |
+    1275             :   // --------------------------------------------------------------
+    1276             : 
+    1277         218 :   std::vector<std::string> custom_trackers;
+    1278             : 
+    1279         109 :   param_loader.loadParam("mrs_trackers", _tracker_names_);
+    1280         109 :   param_loader.loadParam("trackers", custom_trackers);
+    1281             : 
+    1282         109 :   if (!custom_trackers.empty()) {
+    1283           1 :     _tracker_names_.insert(_tracker_names_.end(), custom_trackers.begin(), custom_trackers.end());
+    1284             :   }
+    1285             : 
+    1286         109 :   param_loader.loadParam("null_tracker", _null_tracker_name_);
+    1287         109 :   param_loader.loadParam("landing_takeoff_tracker", _landoff_tracker_name_);
+    1288             : 
+    1289         109 :   tracker_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::Tracker>>("mrs_uav_managers", "mrs_uav_managers::Tracker");
+    1290             : 
+    1291         764 :   for (int i = 0; i < int(_tracker_names_.size()); i++) {
+    1292             : 
+    1293        1310 :     std::string tracker_name = _tracker_names_.at(i);
+    1294             : 
+    1295             :     // load the controller parameters
+    1296        1310 :     std::string address;
+    1297        1310 :     std::string name_space;
+    1298             :     bool        human_switchable;
+    1299             : 
+    1300         655 :     param_loader.loadParam(tracker_name + "/address", address);
+    1301         655 :     param_loader.loadParam(tracker_name + "/namespace", name_space);
+    1302         655 :     param_loader.loadParam(tracker_name + "/human_switchable", human_switchable, false);
+    1303             : 
+    1304        1965 :     TrackerParams new_tracker(address, name_space, human_switchable);
+    1305         655 :     trackers_.insert(std::pair<std::string, TrackerParams>(tracker_name, new_tracker));
+    1306             : 
+    1307             :     try {
+    1308         655 :       ROS_INFO("[ControlManager]: loading the tracker '%s'", new_tracker.address.c_str());
+    1309         655 :       tracker_list_.push_back(tracker_loader_->createInstance(new_tracker.address.c_str()));
+    1310             :     }
+    1311           0 :     catch (pluginlib::CreateClassException& ex1) {
+    1312           0 :       ROS_ERROR("[ControlManager]: CreateClassException for the tracker '%s'", new_tracker.address.c_str());
+    1313           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex1.what());
+    1314           0 :       ros::shutdown();
+    1315             :     }
+    1316           0 :     catch (pluginlib::PluginlibException& ex) {
+    1317           0 :       ROS_ERROR("[ControlManager]: PluginlibException for the tracker '%s'", new_tracker.address.c_str());
+    1318           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex.what());
+    1319           0 :       ros::shutdown();
+    1320             :     }
+    1321             :   }
+    1322             : 
+    1323         109 :   ROS_INFO("[ControlManager]: trackers were loaded");
+    1324             : 
+    1325         764 :   for (int i = 0; i < int(tracker_list_.size()); i++) {
+    1326             : 
+    1327         655 :     std::map<std::string, TrackerParams>::iterator it;
+    1328         655 :     it = trackers_.find(_tracker_names_.at(i));
+    1329             : 
+    1330             :     // create private handlers
+    1331             :     std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers =
+    1332        1310 :         std::make_shared<mrs_uav_managers::control_manager::PrivateHandlers_t>();
+    1333             : 
+    1334         655 :     private_handlers->loadConfigFile = boost::bind(&ControlManager::loadConfigFile, this, _1, it->second.name_space);
+    1335         655 :     private_handlers->name_space     = it->second.name_space;
+    1336         655 :     private_handlers->runtime_name   = _tracker_names_.at(i);
+    1337         655 :     private_handlers->param_loader   = std::make_unique<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, it->second.name_space), _tracker_names_.at(i));
+    1338             : 
+    1339         655 :     if (_custom_config_ != "") {
+    1340         655 :       private_handlers->param_loader->addYamlFile(_custom_config_);
+    1341             :     }
+    1342             : 
+    1343         655 :     if (_platform_config_ != "") {
+    1344         655 :       private_handlers->param_loader->addYamlFile(_platform_config_);
+    1345             :     }
+    1346             : 
+    1347         655 :     if (_world_config_ != "") {
+    1348         655 :       private_handlers->param_loader->addYamlFile(_world_config_);
+    1349             :     }
+    1350             : 
+    1351         655 :     if (_network_config_ != "") {
+    1352         655 :       private_handlers->param_loader->addYamlFile(_network_config_);
+    1353             :     }
+    1354             : 
+    1355         655 :     bool success = false;
+    1356             : 
+    1357             :     try {
+    1358         655 :       ROS_INFO("[ControlManager]: initializing the tracker '%s'", it->second.address.c_str());
+    1359         655 :       success = tracker_list_.at(i)->initialize(ros::NodeHandle(nh_, it->second.name_space), common_handlers_, private_handlers);
+    1360             :     }
+    1361           0 :     catch (std::runtime_error& ex) {
+    1362           0 :       ROS_ERROR("[ControlManager]: exception caught during tracker initialization: '%s'", ex.what());
+    1363             :     }
+    1364             : 
+    1365         655 :     if (!success) {
+    1366           0 :       ROS_ERROR("[ControlManager]: failed to initialize the tracker '%s'", it->second.address.c_str());
+    1367           0 :       ros::shutdown();
+    1368             :     }
+    1369             :   }
+    1370             : 
+    1371         109 :   ROS_INFO("[ControlManager]: trackers were initialized");
+    1372             : 
+    1373             :   // --------------------------------------------------------------
+    1374             :   // |           check the existance of selected trackers         |
+    1375             :   // --------------------------------------------------------------
+    1376             : 
+    1377             :   // | ------ check for the existance of the hover tracker ------ |
+    1378             : 
+    1379             :   // check if the hover_tracker is within the loaded trackers
+    1380             :   {
+    1381         109 :     auto idx = idxInVector(_ehover_tracker_name_, _tracker_names_);
+    1382             : 
+    1383         109 :     if (idx) {
+    1384         109 :       _ehover_tracker_idx_ = idx.value();
+    1385             :     } else {
+    1386           0 :       ROS_ERROR("[ControlManager]: the safety/hover_tracker (%s) is not within the loaded trackers", _ehover_tracker_name_.c_str());
+    1387           0 :       ros::shutdown();
+    1388             :     }
+    1389             :   }
+    1390             : 
+    1391             :   // | ----- check for the existence of the landoff tracker ----- |
+    1392             : 
+    1393             :   {
+    1394         109 :     auto idx = idxInVector(_landoff_tracker_name_, _tracker_names_);
+    1395             : 
+    1396         109 :     if (idx) {
+    1397         109 :       _landoff_tracker_idx_ = idx.value();
+    1398             :     } else {
+    1399           0 :       ROS_ERROR("[ControlManager]: the landoff tracker (%s) is not within the loaded trackers", _landoff_tracker_name_.c_str());
+    1400           0 :       ros::shutdown();
+    1401             :     }
+    1402             :   }
+    1403             : 
+    1404             :   // | ------- check for the existence of the null tracker ------ |
+    1405             : 
+    1406             :   {
+    1407         109 :     auto idx = idxInVector(_null_tracker_name_, _tracker_names_);
+    1408             : 
+    1409         109 :     if (idx) {
+    1410         109 :       _null_tracker_idx_ = idx.value();
+    1411             :     } else {
+    1412           0 :       ROS_ERROR("[ControlManager]: the null tracker (%s) is not within the loaded trackers", _null_tracker_name_.c_str());
+    1413           0 :       ros::shutdown();
+    1414             :     }
+    1415             :   }
+    1416             : 
+    1417             :   // --------------------------------------------------------------
+    1418             :   // |         check existance of trackers for joystick           |
+    1419             :   // --------------------------------------------------------------
+    1420             : 
+    1421         109 :   if (_joystick_enabled_) {
+    1422             : 
+    1423         109 :     auto idx = idxInVector(_joystick_tracker_name_, _tracker_names_);
+    1424             : 
+    1425         109 :     if (idx) {
+    1426         109 :       _joystick_tracker_idx_ = idx.value();
+    1427             :     } else {
+    1428           0 :       ROS_ERROR("[ControlManager]: the joystick tracker (%s) is not within the loaded trackers", _joystick_tracker_name_.c_str());
+    1429           0 :       ros::shutdown();
+    1430             :     }
+    1431             :   }
+    1432             : 
+    1433         109 :   if (_bumper_switch_tracker_) {
+    1434             : 
+    1435         109 :     auto idx = idxInVector(_bumper_tracker_name_, _tracker_names_);
+    1436             : 
+    1437         109 :     if (!idx) {
+    1438           0 :       ROS_ERROR("[ControlManager]: the bumper tracker (%s) is not within the loaded trackers", _bumper_tracker_name_.c_str());
+    1439           0 :       ros::shutdown();
+    1440             :     }
+    1441             :   }
+    1442             : 
+    1443             :   {
+    1444         109 :     auto idx = idxInVector(_joystick_fallback_tracker_name_, _tracker_names_);
+    1445             : 
+    1446         109 :     if (idx) {
+    1447         109 :       _joystick_fallback_tracker_idx_ = idx.value();
+    1448             :     } else {
+    1449           0 :       ROS_ERROR("[ControlManager]: the joystick fallback tracker (%s) is not within the loaded trackers", _joystick_fallback_tracker_name_.c_str());
+    1450           0 :       ros::shutdown();
+    1451             :     }
+    1452             :   }
+    1453             : 
+    1454             :   // --------------------------------------------------------------
+    1455             :   // |                    load the controllers                    |
+    1456             :   // --------------------------------------------------------------
+    1457             : 
+    1458         218 :   std::vector<std::string> custom_controllers;
+    1459             : 
+    1460         109 :   param_loader.loadParam("mrs_controllers", _controller_names_);
+    1461         109 :   param_loader.loadParam("controllers", custom_controllers);
+    1462             : 
+    1463         109 :   if (!custom_controllers.empty()) {
+    1464           0 :     _controller_names_.insert(_controller_names_.end(), custom_controllers.begin(), custom_controllers.end());
+    1465             :   }
+    1466             : 
+    1467         109 :   controller_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::Controller>>("mrs_uav_managers", "mrs_uav_managers::Controller");
+    1468             : 
+    1469             :   // for each controller in the list
+    1470         654 :   for (int i = 0; i < int(_controller_names_.size()); i++) {
+    1471             : 
+    1472        1090 :     std::string controller_name = _controller_names_.at(i);
+    1473             : 
+    1474             :     // load the controller parameters
+    1475        1090 :     std::string address;
+    1476        1090 :     std::string name_space;
+    1477             :     double      eland_threshold, failsafe_threshold, odometry_innovation_threshold;
+    1478             :     bool        human_switchable;
+    1479         545 :     param_loader.loadParam(controller_name + "/address", address);
+    1480         545 :     param_loader.loadParam(controller_name + "/namespace", name_space);
+    1481         545 :     param_loader.loadParam(controller_name + "/eland_threshold", eland_threshold);
+    1482         545 :     param_loader.loadParam(controller_name + "/failsafe_threshold", failsafe_threshold);
+    1483         545 :     param_loader.loadParam(controller_name + "/odometry_innovation_threshold", odometry_innovation_threshold);
+    1484         545 :     param_loader.loadParam(controller_name + "/human_switchable", human_switchable, false);
+    1485             : 
+    1486             :     // check if the controller can output some of the required outputs
+    1487             :     {
+    1488             : 
+    1489         545 :       ControlOutputModalities_t outputs;
+    1490         545 :       param_loader.loadParam(controller_name + "/outputs/actuators", outputs.actuators, false);
+    1491         545 :       param_loader.loadParam(controller_name + "/outputs/control_group", outputs.control_group, false);
+    1492         545 :       param_loader.loadParam(controller_name + "/outputs/attitude_rate", outputs.attitude_rate, false);
+    1493         545 :       param_loader.loadParam(controller_name + "/outputs/attitude", outputs.attitude, false);
+    1494         545 :       param_loader.loadParam(controller_name + "/outputs/acceleration_hdg_rate", outputs.acceleration_hdg_rate, false);
+    1495         545 :       param_loader.loadParam(controller_name + "/outputs/acceleration_hdg", outputs.acceleration_hdg, false);
+    1496         545 :       param_loader.loadParam(controller_name + "/outputs/velocity_hdg_rate", outputs.velocity_hdg_rate, false);
+    1497         545 :       param_loader.loadParam(controller_name + "/outputs/velocity_hdg", outputs.velocity_hdg, false);
+    1498         545 :       param_loader.loadParam(controller_name + "/outputs/position", outputs.position, false);
+    1499             : 
+    1500         545 :       bool meets_actuators             = (_hw_api_inputs_.actuators && outputs.actuators);
+    1501         545 :       bool meets_control_group         = (_hw_api_inputs_.control_group && outputs.control_group);
+    1502         545 :       bool meets_attitude_rate         = (_hw_api_inputs_.attitude_rate && outputs.attitude_rate);
+    1503         545 :       bool meets_attitude              = (_hw_api_inputs_.attitude && outputs.attitude);
+    1504         545 :       bool meets_acceleration_hdg_rate = (_hw_api_inputs_.acceleration_hdg_rate && outputs.acceleration_hdg_rate);
+    1505         545 :       bool meets_acceleration_hdg      = (_hw_api_inputs_.acceleration_hdg && outputs.acceleration_hdg);
+    1506         545 :       bool meets_velocity_hdg_rate     = (_hw_api_inputs_.velocity_hdg_rate && outputs.velocity_hdg_rate);
+    1507         545 :       bool meets_velocity_hdg          = (_hw_api_inputs_.velocity_hdg && outputs.velocity_hdg);
+    1508         545 :       bool meets_position              = (_hw_api_inputs_.position && outputs.position);
+    1509             : 
+    1510         530 :       bool meets_requirements = meets_actuators || meets_control_group || meets_attitude_rate || meets_attitude || meets_acceleration_hdg_rate ||
+    1511        1075 :                                 meets_acceleration_hdg || meets_velocity_hdg_rate || meets_velocity_hdg || meets_position;
+    1512             : 
+    1513         545 :       if (!meets_requirements) {
+    1514             : 
+    1515           0 :         ROS_ERROR("[ControlManager]: the controller '%s' does not meet the control output requirements, which are some of the following",
+    1516             :                   controller_name.c_str());
+    1517             : 
+    1518           0 :         if (_hw_api_inputs_.actuators) {
+    1519           0 :           ROS_ERROR("[ControlManager]: - actuators");
+    1520             :         }
+    1521             : 
+    1522           0 :         if (_hw_api_inputs_.control_group) {
+    1523           0 :           ROS_ERROR("[ControlManager]: - control group");
+    1524             :         }
+    1525             : 
+    1526           0 :         if (_hw_api_inputs_.attitude_rate) {
+    1527           0 :           ROS_ERROR("[ControlManager]: - attitude rate");
+    1528             :         }
+    1529             : 
+    1530           0 :         if (_hw_api_inputs_.attitude) {
+    1531           0 :           ROS_ERROR("[ControlManager]: - attitude");
+    1532             :         }
+    1533             : 
+    1534           0 :         if (_hw_api_inputs_.acceleration_hdg_rate) {
+    1535           0 :           ROS_ERROR("[ControlManager]: - acceleration+hdg rate");
+    1536             :         }
+    1537             : 
+    1538           0 :         if (_hw_api_inputs_.acceleration_hdg) {
+    1539           0 :           ROS_ERROR("[ControlManager]: - acceleration+hdg");
+    1540             :         }
+    1541             : 
+    1542           0 :         if (_hw_api_inputs_.velocity_hdg_rate) {
+    1543           0 :           ROS_ERROR("[ControlManager]: - velocity+hdg rate");
+    1544             :         }
+    1545             : 
+    1546           0 :         if (_hw_api_inputs_.velocity_hdg) {
+    1547           0 :           ROS_ERROR("[ControlManager]: - velocity+hdg");
+    1548             :         }
+    1549             : 
+    1550           0 :         if (_hw_api_inputs_.position) {
+    1551           0 :           ROS_ERROR("[ControlManager]: - position");
+    1552             :         }
+    1553             : 
+    1554           0 :         ros::shutdown();
+    1555             :       }
+    1556             : 
+    1557         545 :       if ((_hw_api_inputs_.actuators || _hw_api_inputs_.control_group) && !common_handlers_->detailed_model_params) {
+    1558           0 :         ROS_ERROR(
+    1559             :             "[ControlManager]: the HW API supports 'actuators' or 'control_group' input, but the 'detailed uav model params' were not loaded sucessfully");
+    1560           0 :         ros::shutdown();
+    1561             :       }
+    1562             :     }
+    1563             : 
+    1564             :     // | --- alter the timer rates based on the hw capabilities --- |
+    1565             : 
+    1566         545 :     CONTROL_OUTPUT lowest_output = getLowestOuput(_hw_api_inputs_);
+    1567             : 
+    1568         545 :     if (lowest_output == ACTUATORS_CMD || lowest_output == CONTROL_GROUP) {
+    1569          30 :       _safety_timer_rate_     = 200.0;
+    1570          30 :       desired_uav_state_rate_ = 250.0;
+    1571         515 :     } else if (lowest_output == ATTITUDE_RATE || lowest_output == ATTITUDE) {
+    1572         425 :       _safety_timer_rate_     = 100.0;
+    1573         425 :       desired_uav_state_rate_ = 100.0;
+    1574          90 :     } else if (lowest_output == ACCELERATION_HDG_RATE || lowest_output == ACCELERATION_HDG) {
+    1575          20 :       _safety_timer_rate_     = 30.0;
+    1576          20 :       _status_timer_rate_     = 1.0;
+    1577          20 :       desired_uav_state_rate_ = 40.0;
+    1578             : 
+    1579          20 :       if (_uav_state_max_missing_time_ < 0.2) {
+    1580           4 :         _uav_state_max_missing_time_ = 0.2;
+    1581             :       }
+    1582          70 :     } else if (lowest_output >= VELOCITY_HDG_RATE) {
+    1583          70 :       _safety_timer_rate_     = 20.0;
+    1584          70 :       _status_timer_rate_     = 1.0;
+    1585          70 :       desired_uav_state_rate_ = 20.0;
+    1586             : 
+    1587          70 :       if (_uav_state_max_missing_time_ < 1.0) {
+    1588          14 :         _uav_state_max_missing_time_ = 1.0;
+    1589             :       }
+    1590             :     }
+    1591             : 
+    1592         545 :     if (eland_threshold == 0) {
+    1593         110 :       eland_threshold = 1e6;
+    1594             :     }
+    1595             : 
+    1596         545 :     if (failsafe_threshold == 0) {
+    1597         110 :       failsafe_threshold = 1e6;
+    1598             :     }
+    1599             : 
+    1600         545 :     if (odometry_innovation_threshold == 0) {
+    1601         111 :       odometry_innovation_threshold = 1e6;
+    1602             :     }
+    1603             : 
+    1604        1635 :     ControllerParams new_controller(address, name_space, eland_threshold, failsafe_threshold, odometry_innovation_threshold, human_switchable);
+    1605         545 :     controllers_.insert(std::pair<std::string, ControllerParams>(controller_name, new_controller));
+    1606             : 
+    1607             :     try {
+    1608         545 :       ROS_INFO("[ControlManager]: loading the controller '%s'", new_controller.address.c_str());
+    1609         545 :       controller_list_.push_back(controller_loader_->createInstance(new_controller.address.c_str()));
+    1610             :     }
+    1611           0 :     catch (pluginlib::CreateClassException& ex1) {
+    1612           0 :       ROS_ERROR("[ControlManager]: CreateClassException for the controller '%s'", new_controller.address.c_str());
+    1613           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex1.what());
+    1614           0 :       ros::shutdown();
+    1615             :     }
+    1616           0 :     catch (pluginlib::PluginlibException& ex) {
+    1617           0 :       ROS_ERROR("[ControlManager]: PluginlibException for the controller '%s'", new_controller.address.c_str());
+    1618           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex.what());
+    1619           0 :       ros::shutdown();
+    1620             :     }
+    1621             :   }
+    1622             : 
+    1623         109 :   ROS_INFO("[ControlManager]: controllers were loaded");
+    1624             : 
+    1625         654 :   for (int i = 0; i < int(controller_list_.size()); i++) {
+    1626             : 
+    1627         545 :     std::map<std::string, ControllerParams>::iterator it;
+    1628         545 :     it = controllers_.find(_controller_names_.at(i));
+    1629             : 
+    1630             :     // create private handlers
+    1631             :     std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers =
+    1632        1090 :         std::make_shared<mrs_uav_managers::control_manager::PrivateHandlers_t>();
+    1633             : 
+    1634         545 :     private_handlers->loadConfigFile = boost::bind(&ControlManager::loadConfigFile, this, _1, it->second.name_space);
+    1635         545 :     private_handlers->name_space     = it->second.name_space;
+    1636         545 :     private_handlers->runtime_name   = _controller_names_.at(i);
+    1637         545 :     private_handlers->param_loader   = std::make_unique<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, it->second.name_space), _controller_names_.at(i));
+    1638             : 
+    1639         545 :     if (_custom_config_ != "") {
+    1640         545 :       private_handlers->param_loader->addYamlFile(_custom_config_);
+    1641             :     }
+    1642             : 
+    1643         545 :     if (_platform_config_ != "") {
+    1644         545 :       private_handlers->param_loader->addYamlFile(_platform_config_);
+    1645             :     }
+    1646             : 
+    1647         545 :     if (_world_config_ != "") {
+    1648         545 :       private_handlers->param_loader->addYamlFile(_world_config_);
+    1649             :     }
+    1650             : 
+    1651         545 :     if (_network_config_ != "") {
+    1652         545 :       private_handlers->param_loader->addYamlFile(_network_config_);
+    1653             :     }
+    1654             : 
+    1655         545 :     bool success = false;
+    1656             : 
+    1657             :     try {
+    1658             : 
+    1659         545 :       ROS_INFO("[ControlManager]: initializing the controller '%s'", it->second.address.c_str());
+    1660         545 :       success = controller_list_.at(i)->initialize(ros::NodeHandle(nh_, it->second.name_space), common_handlers_, private_handlers);
+    1661             :     }
+    1662           0 :     catch (std::runtime_error& ex) {
+    1663           0 :       ROS_ERROR("[ControlManager]: exception caught during controller initialization: '%s'", ex.what());
+    1664             :     }
+    1665             : 
+    1666         545 :     if (!success) {
+    1667           0 :       ROS_ERROR("[ControlManager]: failed to initialize the controller '%s'", it->second.address.c_str());
+    1668           0 :       ros::shutdown();
+    1669             :     }
+    1670             :   }
+    1671             : 
+    1672         109 :   ROS_INFO("[ControlManager]: controllers were initialized");
+    1673             : 
+    1674             :   {
+    1675         109 :     auto idx = idxInVector(_failsafe_controller_name_, _controller_names_);
+    1676             : 
+    1677         109 :     if (idx) {
+    1678         109 :       _failsafe_controller_idx_ = idx.value();
+    1679             :     } else {
+    1680           0 :       ROS_ERROR("[ControlManager]: the failsafe controller (%s) is not within the loaded controllers", _failsafe_controller_name_.c_str());
+    1681           0 :       ros::shutdown();
+    1682             :     }
+    1683             :   }
+    1684             : 
+    1685             :   {
+    1686         109 :     auto idx = idxInVector(_eland_controller_name_, _controller_names_);
+    1687             : 
+    1688         109 :     if (idx) {
+    1689         109 :       _eland_controller_idx_ = idx.value();
+    1690             :     } else {
+    1691           0 :       ROS_ERROR("[ControlManager]: the eland controller (%s) is not within the loaded controllers", _eland_controller_name_.c_str());
+    1692           0 :       ros::shutdown();
+    1693             :     }
+    1694             :   }
+    1695             : 
+    1696             :   {
+    1697         109 :     auto idx = idxInVector(_joystick_controller_name_, _controller_names_);
+    1698             : 
+    1699         109 :     if (idx) {
+    1700         109 :       _joystick_controller_idx_ = idx.value();
+    1701             :     } else {
+    1702           0 :       ROS_ERROR("[ControlManager]: the joystick controller (%s) is not within the loaded controllers", _joystick_controller_name_.c_str());
+    1703           0 :       ros::shutdown();
+    1704             :     }
+    1705             :   }
+    1706             : 
+    1707         109 :   if (_bumper_switch_controller_) {
+    1708             : 
+    1709         109 :     auto idx = idxInVector(_bumper_controller_name_, _controller_names_);
+    1710             : 
+    1711         109 :     if (!idx) {
+    1712           0 :       ROS_ERROR("[ControlManager]: the bumper controller (%s) is not within the loaded controllers", _bumper_controller_name_.c_str());
+    1713           0 :       ros::shutdown();
+    1714             :     }
+    1715             :   }
+    1716             : 
+    1717             :   {
+    1718         109 :     auto idx = idxInVector(_joystick_fallback_controller_name_, _controller_names_);
+    1719             : 
+    1720         109 :     if (idx) {
+    1721         109 :       _joystick_fallback_controller_idx_ = idx.value();
+    1722             :     } else {
+    1723           0 :       ROS_ERROR("[ControlManager]: the joystick fallback controller (%s) is not within the loaded controllers", _joystick_fallback_controller_name_.c_str());
+    1724           0 :       ros::shutdown();
+    1725             :     }
+    1726             :   }
+    1727             : 
+    1728             :   // --------------------------------------------------------------
+    1729             :   // |                  activate the NullTracker                  |
+    1730             :   // --------------------------------------------------------------
+    1731             : 
+    1732         109 :   ROS_INFO("[ControlManager]: activating the null tracker");
+    1733             : 
+    1734         109 :   tracker_list_.at(_null_tracker_idx_)->activate(last_tracker_cmd_);
+    1735         109 :   active_tracker_idx_ = _null_tracker_idx_;
+    1736             : 
+    1737             :   // --------------------------------------------------------------
+    1738             :   // |    activate the eland controller as the first controller   |
+    1739             :   // --------------------------------------------------------------
+    1740             : 
+    1741         109 :   ROS_INFO("[ControlManager]: activating the the eland controller (%s) as the first controller", _controller_names_.at(_eland_controller_idx_).c_str());
+    1742             : 
+    1743         109 :   controller_list_.at(_eland_controller_idx_)->activate(last_control_output_);
+    1744         109 :   active_controller_idx_ = _eland_controller_idx_;
+    1745             : 
+    1746             :   // update the time
+    1747             :   {
+    1748         218 :     std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    1749             : 
+    1750         109 :     controller_tracker_switch_time_ = ros::Time::now();
+    1751             :   }
+    1752             : 
+    1753         109 :   output_enabled_ = false;
+    1754             : 
+    1755             :   // | --------------- set the default constraints -------------- |
+    1756             : 
+    1757         109 :   sanitized_constraints_ = current_constraints_;
+    1758         109 :   setConstraints(current_constraints_);
+    1759             : 
+    1760             :   // | ------------------------ profiler ------------------------ |
+    1761             : 
+    1762         109 :   profiler_ = mrs_lib::Profiler(nh_, "ControlManager", _profiler_enabled_);
+    1763             : 
+    1764             :   // | ----------------------- publishers ----------------------- |
+    1765             : 
+    1766         109 :   control_output_publisher_ = OutputPublisher(nh_);
+    1767             : 
+    1768         109 :   ph_controller_diagnostics_             = mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics>(nh_, "controller_diagnostics_out", 1);
+    1769         109 :   ph_tracker_cmd_                        = mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand>(nh_, "tracker_cmd_out", 1);
+    1770         109 :   ph_mrs_odom_input_                     = mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput>(nh_, "estimator_input_out", 1);
+    1771         109 :   ph_control_reference_odom_             = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh_, "control_reference_out", 1);
+    1772         109 :   ph_diagnostics_                        = mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics>(nh_, "diagnostics_out", 1);
+    1773         109 :   ph_offboard_on_                        = mrs_lib::PublisherHandler<std_msgs::Empty>(nh_, "offboard_on_out", 1);
+    1774         109 :   ph_tilt_error_                         = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "tilt_error_out", 1);
+    1775         109 :   ph_mass_estimate_                      = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "mass_estimate_out", 1, false, 10.0);
+    1776         109 :   ph_mass_nominal_                       = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "mass_nominal_out", 1, true);
+    1777         109 :   ph_throttle_                           = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "throttle_out", 1, false, 10.0);
+    1778         109 :   ph_thrust_                             = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "thrust_out", 1, false, 100.0);
+    1779         109 :   ph_control_error_                      = mrs_lib::PublisherHandler<mrs_msgs::ControlError>(nh_, "control_error_out", 1);
+    1780         109 :   ph_safety_area_markers_                = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "safety_area_markers_out", 1, true, 1.0);
+    1781         109 :   ph_safety_area_coordinates_markers_    = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "safety_area_coordinates_markers_out", 1, true, 1.0);
+    1782         109 :   ph_disturbances_markers_               = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "disturbances_markers_out", 1, false, 10.0);
+    1783         109 :   ph_current_constraints_                = mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints>(nh_, "current_constraints_out", 1);
+    1784         109 :   ph_heading_                            = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "heading_out", 1);
+    1785         109 :   ph_speed_                              = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "speed_out", 1, false, 10.0);
+    1786         109 :   pub_debug_original_trajectory_poses_   = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "trajectory_original/poses_out", 1, true);
+    1787         109 :   pub_debug_original_trajectory_markers_ = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "trajectory_original/markers_out", 1, true);
+    1788             : 
+    1789             :   // | ------------------ publish nominal mass ------------------ |
+    1790             : 
+    1791             :   {
+    1792         109 :     std_msgs::Float64 nominal_mass;
+    1793             : 
+    1794         109 :     nominal_mass.data = _uav_mass_;
+    1795             : 
+    1796         109 :     ph_mass_nominal_.publish(nominal_mass);
+    1797             :   }
+    1798             : 
+    1799             :   // | ----------------------- subscribers ---------------------- |
+    1800             : 
+    1801         218 :   mrs_lib::SubscribeHandlerOptions shopts;
+    1802         109 :   shopts.nh                 = nh_;
+    1803         109 :   shopts.node_name          = "ControlManager";
+    1804         109 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+    1805         109 :   shopts.threadsafe         = true;
+    1806         109 :   shopts.autostart          = true;
+    1807         109 :   shopts.queue_size         = 10;
+    1808         109 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+    1809             : 
+    1810         109 :   if (_state_input_ == INPUT_UAV_STATE) {
+    1811         109 :     sh_uav_state_ = mrs_lib::SubscribeHandler<mrs_msgs::UavState>(shopts, "uav_state_in", &ControlManager::callbackUavState, this);
+    1812           0 :   } else if (_state_input_ == INPUT_ODOMETRY) {
+    1813           0 :     sh_odometry_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, "odometry_in", &ControlManager::callbackOdometry, this);
+    1814             :   }
+    1815             : 
+    1816         109 :   if (_odometry_innovation_check_enabled_) {
+    1817         109 :     sh_odometry_innovation_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, "odometry_innovation_in");
+    1818             :   }
+    1819             : 
+    1820         109 :   sh_bumper_    = mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors>(shopts, "bumper_sectors_in");
+    1821         109 :   sh_max_z_     = mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>(shopts, "max_z_in");
+    1822         109 :   sh_joystick_  = mrs_lib::SubscribeHandler<sensor_msgs::Joy>(shopts, "joystick_in", &ControlManager::callbackJoystick, this);
+    1823         109 :   sh_gnss_      = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, "gnss_in", &ControlManager::callbackGNSS, this);
+    1824         109 :   sh_hw_api_rc_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels>(shopts, "hw_api_rc_in", &ControlManager::callbackRC, this);
+    1825             : 
+    1826         109 :   sh_hw_api_status_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus>(shopts, "hw_api_status_in", &ControlManager::callbackHwApiStatus, this);
+    1827             : 
+    1828             :   // | -------------------- general services -------------------- |
+    1829             : 
+    1830         109 :   service_server_switch_tracker_             = nh_.advertiseService("switch_tracker_in", &ControlManager::callbackSwitchTracker, this);
+    1831         109 :   service_server_switch_controller_          = nh_.advertiseService("switch_controller_in", &ControlManager::callbackSwitchController, this);
+    1832         109 :   service_server_reset_tracker_              = nh_.advertiseService("tracker_reset_static_in", &ControlManager::callbackTrackerResetStatic, this);
+    1833         109 :   service_server_hover_                      = nh_.advertiseService("hover_in", &ControlManager::callbackHover, this);
+    1834         109 :   service_server_ehover_                     = nh_.advertiseService("ehover_in", &ControlManager::callbackEHover, this);
+    1835         109 :   service_server_failsafe_                   = nh_.advertiseService("failsafe_in", &ControlManager::callbackFailsafe, this);
+    1836         109 :   service_server_failsafe_escalating_        = nh_.advertiseService("failsafe_escalating_in", &ControlManager::callbackFailsafeEscalating, this);
+    1837         109 :   service_server_toggle_output_              = nh_.advertiseService("toggle_output_in", &ControlManager::callbackToggleOutput, this);
+    1838         109 :   service_server_arm_                        = nh_.advertiseService("arm_in", &ControlManager::callbackArm, this);
+    1839         109 :   service_server_enable_callbacks_           = nh_.advertiseService("enable_callbacks_in", &ControlManager::callbackEnableCallbacks, this);
+    1840         109 :   service_server_set_constraints_            = nh_.advertiseService("set_constraints_in", &ControlManager::callbackSetConstraints, this);
+    1841         109 :   service_server_use_joystick_               = nh_.advertiseService("use_joystick_in", &ControlManager::callbackUseJoystick, this);
+    1842         109 :   service_server_use_safety_area_            = nh_.advertiseService("use_safety_area_in", &ControlManager::callbackUseSafetyArea, this);
+    1843         109 :   service_server_eland_                      = nh_.advertiseService("eland_in", &ControlManager::callbackEland, this);
+    1844         109 :   service_server_parachute_                  = nh_.advertiseService("parachute_in", &ControlManager::callbackParachute, this);
+    1845         109 :   service_server_set_min_z_                  = nh_.advertiseService("set_min_z_in", &ControlManager::callbackSetMinZ, this);
+    1846         109 :   service_server_transform_reference_        = nh_.advertiseService("transform_reference_in", &ControlManager::callbackTransformReference, this);
+    1847         109 :   service_server_transform_reference_array_   = nh_.advertiseService("transform_reference_array_in", &ControlManager::callbackTransformReferenceArray, this);
+    1848         109 :   service_server_transform_pose_             = nh_.advertiseService("transform_pose_in", &ControlManager::callbackTransformPose, this);
+    1849         109 :   service_server_transform_vector3_          = nh_.advertiseService("transform_vector3_in", &ControlManager::callbackTransformVector3, this);
+    1850         109 :   service_server_bumper_enabler_             = nh_.advertiseService("bumper_in", &ControlManager::callbackEnableBumper, this);
+    1851         109 :   service_server_get_min_z_                  = nh_.advertiseService("get_min_z_in", &ControlManager::callbackGetMinZ, this);
+    1852         109 :   service_server_validate_reference_         = nh_.advertiseService("validate_reference_in", &ControlManager::callbackValidateReference, this);
+    1853         109 :   service_server_validate_reference_2d_      = nh_.advertiseService("validate_reference_2d_in", &ControlManager::callbackValidateReference2d, this);
+    1854         109 :   service_server_validate_reference_array_    = nh_.advertiseService("validate_reference_array_in", &ControlManager::callbackValidateReferenceArray, this);
+    1855         109 :   service_server_start_trajectory_tracking_  = nh_.advertiseService("start_trajectory_tracking_in", &ControlManager::callbackStartTrajectoryTracking, this);
+    1856         109 :   service_server_stop_trajectory_tracking_   = nh_.advertiseService("stop_trajectory_tracking_in", &ControlManager::callbackStopTrajectoryTracking, this);
+    1857         109 :   service_server_resume_trajectory_tracking_ = nh_.advertiseService("resume_trajectory_tracking_in", &ControlManager::callbackResumeTrajectoryTracking, this);
+    1858         109 :   service_server_goto_trajectory_start_      = nh_.advertiseService("goto_trajectory_start_in", &ControlManager::callbackGotoTrajectoryStart, this);
+    1859             : 
+    1860         109 :   sch_arming_                 = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "hw_api_arming_out");
+    1861         109 :   sch_eland_                  = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "eland_out");
+    1862         109 :   sch_shutdown_               = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "shutdown_out");
+    1863         109 :   sch_set_odometry_callbacks_ = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "set_odometry_callbacks_out");
+    1864         109 :   sch_ungrip_                 = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "ungrip_out");
+    1865         109 :   sch_parachute_              = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "parachute_out");
+    1866             : 
+    1867             :   // | ---------------- setpoint command services --------------- |
+    1868             : 
+    1869             :   // human callable
+    1870         109 :   service_server_goto_                 = nh_.advertiseService("goto_in", &ControlManager::callbackGoto, this);
+    1871         109 :   service_server_goto_fcu_             = nh_.advertiseService("goto_fcu_in", &ControlManager::callbackGotoFcu, this);
+    1872         109 :   service_server_goto_relative_        = nh_.advertiseService("goto_relative_in", &ControlManager::callbackGotoRelative, this);
+    1873         109 :   service_server_goto_altitude_        = nh_.advertiseService("goto_altitude_in", &ControlManager::callbackGotoAltitude, this);
+    1874         109 :   service_server_set_heading_          = nh_.advertiseService("set_heading_in", &ControlManager::callbackSetHeading, this);
+    1875         109 :   service_server_set_heading_relative_ = nh_.advertiseService("set_heading_relative_in", &ControlManager::callbackSetHeadingRelative, this);
+    1876             : 
+    1877         109 :   service_server_reference_ = nh_.advertiseService("reference_in", &ControlManager::callbackReferenceService, this);
+    1878         109 :   sh_reference_             = mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped>(shopts, "reference_in", &ControlManager::callbackReferenceTopic, this);
+    1879             : 
+    1880         109 :   service_server_velocity_reference_ = nh_.advertiseService("velocity_reference_in", &ControlManager::callbackVelocityReferenceService, this);
+    1881             :   sh_velocity_reference_ =
+    1882         109 :       mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped>(shopts, "velocity_reference_in", &ControlManager::callbackVelocityReferenceTopic, this);
+    1883             : 
+    1884         109 :   service_server_trajectory_reference_ = nh_.advertiseService("trajectory_reference_in", &ControlManager::callbackTrajectoryReferenceService, this);
+    1885             :   sh_trajectory_reference_ =
+    1886         109 :       mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference>(shopts, "trajectory_reference_in", &ControlManager::callbackTrajectoryReferenceTopic, this);
+    1887             : 
+    1888             :   // | --------------------- other services --------------------- |
+    1889             : 
+    1890         109 :   service_server_emergency_reference_ = nh_.advertiseService("emergency_reference_in", &ControlManager::callbackEmergencyReference, this);
+    1891         109 :   service_server_pirouette_           = nh_.advertiseService("pirouette_in", &ControlManager::callbackPirouette, this);
+    1892             : 
+    1893             :   // | ------------------------- timers ------------------------- |
+    1894             : 
+    1895         109 :   timer_status_    = nh_.createTimer(ros::Rate(_status_timer_rate_), &ControlManager::timerStatus, this);
+    1896         109 :   timer_safety_    = nh_.createTimer(ros::Rate(_safety_timer_rate_), &ControlManager::timerSafety, this);
+    1897         109 :   timer_bumper_    = nh_.createTimer(ros::Rate(1.0), &ControlManager::timerBumper, this);
+    1898         109 :   timer_eland_     = nh_.createTimer(ros::Rate(_elanding_timer_rate_), &ControlManager::timerEland, this, false, false);
+    1899         109 :   timer_failsafe_  = nh_.createTimer(ros::Rate(_failsafe_timer_rate_), &ControlManager::timerFailsafe, this, false, false);
+    1900         109 :   timer_pirouette_ = nh_.createTimer(ros::Rate(_pirouette_timer_rate_), &ControlManager::timerPirouette, this, false, false);
+    1901         109 :   timer_joystick_  = nh_.createTimer(ros::Rate(_joystick_timer_rate_), &ControlManager::timerJoystick, this);
+    1902             : 
+    1903             :   // | ----------------------- finish init ---------------------- |
+    1904             : 
+    1905         109 :   if (!param_loader.loadedSuccessfully()) {
+    1906           0 :     ROS_ERROR("[ControlManager]: could not load all parameters!");
+    1907           0 :     ros::shutdown();
+    1908             :   }
+    1909             : 
+    1910         109 :   is_initialized_ = true;
+    1911             : 
+    1912         109 :   ROS_INFO("[ControlManager]: initialized");
+    1913         109 : }
+    1914             : 
+    1915             : //}
+    1916             : 
+    1917             : // --------------------------------------------------------------
+    1918             : // |                           timers                           |
+    1919             : // --------------------------------------------------------------
+    1920             : 
+    1921             : /* timerHwApiCapabilities() //{ */
+    1922             : 
+    1923         185 : void ControlManager::timerHwApiCapabilities(const ros::TimerEvent& event) {
+    1924             : 
+    1925         370 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerHwApiCapabilities", _status_timer_rate_, 1.0, event);
+    1926         370 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerHwApiCapabilities", scope_timer_logger_, scope_timer_enabled_);
+    1927             : 
+    1928         185 :   if (!sh_hw_api_capabilities_.hasMsg()) {
+    1929          76 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: waiting for HW API capabilities");
+    1930          76 :     return;
+    1931             :   }
+    1932             : 
+    1933         218 :   auto hw_ap_capabilities = sh_hw_api_capabilities_.getMsg();
+    1934             : 
+    1935         109 :   ROS_INFO("[ControlManager]: got HW API capabilities, the possible control modes are:");
+    1936             : 
+    1937         109 :   if (hw_ap_capabilities->accepts_actuator_cmd) {
+    1938           3 :     ROS_INFO("[ControlManager]: - actuator command");
+    1939           3 :     _hw_api_inputs_.actuators = true;
+    1940             :   }
+    1941             : 
+    1942         109 :   if (hw_ap_capabilities->accepts_control_group_cmd) {
+    1943           3 :     ROS_INFO("[ControlManager]: - control group command");
+    1944           3 :     _hw_api_inputs_.control_group = true;
+    1945             :   }
+    1946             : 
+    1947         109 :   if (hw_ap_capabilities->accepts_attitude_rate_cmd) {
+    1948          82 :     ROS_INFO("[ControlManager]: - attitude rate command");
+    1949          82 :     _hw_api_inputs_.attitude_rate = true;
+    1950             :   }
+    1951             : 
+    1952         109 :   if (hw_ap_capabilities->accepts_attitude_cmd) {
+    1953          80 :     ROS_INFO("[ControlManager]: - attitude command");
+    1954          80 :     _hw_api_inputs_.attitude = true;
+    1955             :   }
+    1956             : 
+    1957         109 :   if (hw_ap_capabilities->accepts_acceleration_hdg_rate_cmd) {
+    1958           2 :     ROS_INFO("[ControlManager]: - acceleration+hdg rate command");
+    1959           2 :     _hw_api_inputs_.acceleration_hdg_rate = true;
+    1960             :   }
+    1961             : 
+    1962         109 :   if (hw_ap_capabilities->accepts_acceleration_hdg_cmd) {
+    1963           2 :     ROS_INFO("[ControlManager]: - acceleration+hdg command");
+    1964           2 :     _hw_api_inputs_.acceleration_hdg = true;
+    1965             :   }
+    1966             : 
+    1967         109 :   if (hw_ap_capabilities->accepts_velocity_hdg_rate_cmd) {
+    1968           8 :     ROS_INFO("[ControlManager]: - velocityhdg rate command");
+    1969           8 :     _hw_api_inputs_.velocity_hdg_rate = true;
+    1970             :   }
+    1971             : 
+    1972         109 :   if (hw_ap_capabilities->accepts_velocity_hdg_cmd) {
+    1973           4 :     ROS_INFO("[ControlManager]: - velocityhdg command");
+    1974           4 :     _hw_api_inputs_.velocity_hdg = true;
+    1975             :   }
+    1976             : 
+    1977         109 :   if (hw_ap_capabilities->accepts_position_cmd) {
+    1978           2 :     ROS_INFO("[ControlManager]: - position command");
+    1979           2 :     _hw_api_inputs_.position = true;
+    1980             :   }
+    1981             : 
+    1982         109 :   initialize();
+    1983             : 
+    1984         109 :   timer_hw_api_capabilities_.stop();
+    1985             : }
+    1986             : 
+    1987             : //}
+    1988             : 
+    1989             : /* //{ timerStatus() */
+    1990             : 
+    1991       19436 : void ControlManager::timerStatus(const ros::TimerEvent& event) {
+    1992             : 
+    1993       19436 :   if (!is_initialized_) {
+    1994           0 :     return;
+    1995             :   }
+    1996             : 
+    1997       58308 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerStatus", _status_timer_rate_, 0.1, event);
+    1998       58308 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerStatus", scope_timer_logger_, scope_timer_enabled_);
+    1999             : 
+    2000             :   // copy member variables
+    2001       38872 :   auto uav_state             = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    2002       38872 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    2003       19436 :   auto yaw_error             = mrs_lib::get_mutexed(mutex_attitude_error_, yaw_error_);
+    2004       19436 :   auto position_error        = mrs_lib::get_mutexed(mutex_position_error_, position_error_);
+    2005       19436 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    2006       19436 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    2007             : 
+    2008             :   double uav_x, uav_y, uav_z;
+    2009       19436 :   uav_x = uav_state.pose.position.x;
+    2010       19436 :   uav_y = uav_state.pose.position.y;
+    2011       19436 :   uav_z = uav_state.pose.position.z;
+    2012             : 
+    2013             :   // --------------------------------------------------------------
+    2014             :   // |                      print the status                      |
+    2015             :   // --------------------------------------------------------------
+    2016             : 
+    2017             :   {
+    2018       38872 :     std::string controller = _controller_names_.at(active_controller_idx);
+    2019       38872 :     std::string tracker    = _tracker_names_.at(active_tracker_idx);
+    2020       19436 :     double      mass       = last_control_output.diagnostics.total_mass;
+    2021       19436 :     double      bx_b       = last_control_output.diagnostics.disturbance_bx_b;
+    2022       19436 :     double      by_b       = last_control_output.diagnostics.disturbance_by_b;
+    2023       19436 :     double      wx_w       = last_control_output.diagnostics.disturbance_wx_w;
+    2024       19436 :     double      wy_w       = last_control_output.diagnostics.disturbance_wy_w;
+    2025             : 
+    2026       19436 :     ROS_INFO_THROTTLE(5.0, "[ControlManager]: tracker: '%s', controller: '%s', mass: '%.2f kg', disturbances: body [%.2f, %.2f] N, world [%.2f, %.2f] N",
+    2027             :                       tracker.c_str(), controller.c_str(), mass, bx_b, by_b, wx_w, wy_w);
+    2028             :   }
+    2029             : 
+    2030             :   // --------------------------------------------------------------
+    2031             :   // |                   publish the diagnostics                  |
+    2032             :   // --------------------------------------------------------------
+    2033             : 
+    2034       19436 :   publishDiagnostics();
+    2035             : 
+    2036             :   // --------------------------------------------------------------
+    2037             :   // |                publish if the offboard is on               |
+    2038             :   // --------------------------------------------------------------
+    2039             : 
+    2040       19436 :   if (offboard_mode_) {
+    2041             : 
+    2042       13140 :     std_msgs::Empty offboard_on_out;
+    2043             : 
+    2044       13140 :     ph_offboard_on_.publish(offboard_on_out);
+    2045             :   }
+    2046             : 
+    2047             :   // --------------------------------------------------------------
+    2048             :   // |                   publish the tilt error                   |
+    2049             :   // --------------------------------------------------------------
+    2050             :   {
+    2051       38872 :     std::scoped_lock lock(mutex_attitude_error_);
+    2052             : 
+    2053       19436 :     if (tilt_error_) {
+    2054             : 
+    2055       38872 :       mrs_msgs::Float64Stamped tilt_error_out;
+    2056       19436 :       tilt_error_out.header.stamp    = ros::Time::now();
+    2057       19436 :       tilt_error_out.header.frame_id = uav_state.header.frame_id;
+    2058       19436 :       tilt_error_out.value           = (180.0 / M_PI) * tilt_error_.value();
+    2059             : 
+    2060       19436 :       ph_tilt_error_.publish(tilt_error_out);
+    2061             :     }
+    2062             :   }
+    2063             : 
+    2064             :   // --------------------------------------------------------------
+    2065             :   // |                  publish the control error                 |
+    2066             :   // --------------------------------------------------------------
+    2067             : 
+    2068       19436 :   if (position_error) {
+    2069             : 
+    2070       11991 :     Eigen::Vector3d pos_error_value = position_error.value();
+    2071             : 
+    2072       23982 :     mrs_msgs::ControlError msg_out;
+    2073             : 
+    2074       11991 :     msg_out.header.stamp    = ros::Time::now();
+    2075       11991 :     msg_out.header.frame_id = uav_state.header.frame_id;
+    2076             : 
+    2077       11991 :     msg_out.position_errors.x    = pos_error_value(0);
+    2078       11991 :     msg_out.position_errors.y    = pos_error_value(1);
+    2079       11991 :     msg_out.position_errors.z    = pos_error_value(2);
+    2080       11991 :     msg_out.total_position_error = pos_error_value.norm();
+    2081             : 
+    2082       11991 :     if (yaw_error_) {
+    2083       11991 :       msg_out.yaw_error = yaw_error.value();
+    2084             :     }
+    2085             : 
+    2086       11991 :     std::map<std::string, ControllerParams>::iterator it;
+    2087             : 
+    2088       11991 :     it = controllers_.find(_controller_names_.at(active_controller_idx));
+    2089             : 
+    2090       11991 :     msg_out.position_eland_threshold    = it->second.eland_threshold;
+    2091       11991 :     msg_out.position_failsafe_threshold = it->second.failsafe_threshold;
+    2092             : 
+    2093       11991 :     ph_control_error_.publish(msg_out);
+    2094             :   }
+    2095             : 
+    2096             :   // --------------------------------------------------------------
+    2097             :   // |                  publish the mass estimate                 |
+    2098             :   // --------------------------------------------------------------
+    2099             : 
+    2100       19436 :   if (last_control_output.diagnostics.mass_estimator) {
+    2101             : 
+    2102       10651 :     std_msgs::Float64 mass_estimate_out;
+    2103       10651 :     mass_estimate_out.data = _uav_mass_ + last_control_output.diagnostics.mass_difference;
+    2104             : 
+    2105       10651 :     ph_mass_estimate_.publish(mass_estimate_out);
+    2106             :   }
+    2107             : 
+    2108             :   // --------------------------------------------------------------
+    2109             :   // |                 publish the current heading                |
+    2110             :   // --------------------------------------------------------------
+    2111             : 
+    2112       19436 :   if (_state_input_ == INPUT_UAV_STATE && sh_uav_state_.hasMsg()) {
+    2113             : 
+    2114             :     try {
+    2115             : 
+    2116             :       double heading;
+    2117             : 
+    2118       16312 :       heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    2119             : 
+    2120       32624 :       mrs_msgs::Float64Stamped heading_out;
+    2121       16312 :       heading_out.header = uav_state.header;
+    2122       16312 :       heading_out.value  = heading;
+    2123             : 
+    2124       16312 :       ph_heading_.publish(heading_out);
+    2125             :     }
+    2126           0 :     catch (...) {
+    2127           0 :       ROS_ERROR_THROTTLE(1.0, "exception caught, could not transform heading");
+    2128             :     }
+    2129             :   }
+    2130             : 
+    2131             :   // --------------------------------------------------------------
+    2132             :   // |                  publish the current speed                 |
+    2133             :   // --------------------------------------------------------------
+    2134             : 
+    2135       19436 :   if (_state_input_ == INPUT_UAV_STATE && sh_uav_state_.hasMsg()) {
+    2136             : 
+    2137       16312 :     double speed = sqrt(pow(uav_state.velocity.linear.x, 2) + pow(uav_state.velocity.linear.y, 2) + pow(uav_state.velocity.linear.z, 2));
+    2138             : 
+    2139       32624 :     mrs_msgs::Float64Stamped speed_out;
+    2140       16312 :     speed_out.header = uav_state.header;
+    2141       16312 :     speed_out.value  = speed;
+    2142             : 
+    2143       16312 :     ph_speed_.publish(speed_out);
+    2144             :   }
+    2145             : 
+    2146             :   // --------------------------------------------------------------
+    2147             :   // |               publish the safety area markers              |
+    2148             :   // --------------------------------------------------------------
+    2149             : 
+    2150       19436 :   if (use_safety_area_) {
+    2151             : 
+    2152       31144 :     mrs_msgs::ReferenceStamped temp_ref;
+    2153       15572 :     temp_ref.header.frame_id = _safety_area_horizontal_frame_;
+    2154             : 
+    2155       31144 :     geometry_msgs::TransformStamped tf;
+    2156             : 
+    2157       46716 :     auto ret = transformer_->getTransform(_safety_area_horizontal_frame_, "local_origin", ros::Time(0));
+    2158             : 
+    2159       15572 :     if (ret) {
+    2160             : 
+    2161       12804 :       ROS_INFO_ONCE("[ControlManager]: got TFs, publishing safety area markers");
+    2162             : 
+    2163       25608 :       visualization_msgs::MarkerArray safety_area_marker_array;
+    2164       25608 :       visualization_msgs::MarkerArray safety_area_coordinates_marker_array;
+    2165             : 
+    2166       25608 :       mrs_lib::Polygon border = safety_zone_->getBorder();
+    2167             : 
+    2168       25608 :       std::vector<geometry_msgs::Point> border_points_bot_original = border.getPointMessageVector(getMinZ(_safety_area_horizontal_frame_));
+    2169       25608 :       std::vector<geometry_msgs::Point> border_points_top_original = border.getPointMessageVector(getMaxZ(_safety_area_horizontal_frame_));
+    2170             : 
+    2171       25608 :       std::vector<geometry_msgs::Point> border_points_bot_transformed = border_points_bot_original;
+    2172       25608 :       std::vector<geometry_msgs::Point> border_points_top_transformed = border_points_bot_original;
+    2173             : 
+    2174             :       // if we fail in transforming the area at some point
+    2175             :       // do not publish it at all
+    2176       12804 :       bool tf_success = true;
+    2177             : 
+    2178       25608 :       geometry_msgs::TransformStamped tf = ret.value();
+    2179             : 
+    2180             :       /* transform area points to local origin //{ */
+    2181             : 
+    2182             :       // transform border bottom points to local origin
+    2183       64020 :       for (size_t i = 0; i < border_points_bot_original.size(); i++) {
+    2184             : 
+    2185       51216 :         temp_ref.header.frame_id      = _safety_area_horizontal_frame_;
+    2186       51216 :         temp_ref.header.stamp         = ros::Time(0);
+    2187       51216 :         temp_ref.reference.position.x = border_points_bot_original.at(i).x;
+    2188       51216 :         temp_ref.reference.position.y = border_points_bot_original.at(i).y;
+    2189       51216 :         temp_ref.reference.position.z = border_points_bot_original.at(i).z;
+    2190             : 
+    2191      102432 :         if (auto ret = transformer_->transform(temp_ref, tf)) {
+    2192             : 
+    2193       51216 :           temp_ref = ret.value();
+    2194             : 
+    2195       51216 :           border_points_bot_transformed.at(i).x = temp_ref.reference.position.x;
+    2196       51216 :           border_points_bot_transformed.at(i).y = temp_ref.reference.position.y;
+    2197       51216 :           border_points_bot_transformed.at(i).z = temp_ref.reference.position.z;
+    2198             : 
+    2199             :         } else {
+    2200           0 :           tf_success = false;
+    2201             :         }
+    2202             :       }
+    2203             : 
+    2204             :       // transform border top points to local origin
+    2205       64020 :       for (size_t i = 0; i < border_points_top_original.size(); i++) {
+    2206             : 
+    2207       51216 :         temp_ref.header.frame_id      = _safety_area_horizontal_frame_;
+    2208       51216 :         temp_ref.header.stamp         = ros::Time(0);
+    2209       51216 :         temp_ref.reference.position.x = border_points_top_original.at(i).x;
+    2210       51216 :         temp_ref.reference.position.y = border_points_top_original.at(i).y;
+    2211       51216 :         temp_ref.reference.position.z = border_points_top_original.at(i).z;
+    2212             : 
+    2213      102432 :         if (auto ret = transformer_->transform(temp_ref, tf)) {
+    2214             : 
+    2215       51216 :           temp_ref = ret.value();
+    2216             : 
+    2217       51216 :           border_points_top_transformed.at(i).x = temp_ref.reference.position.x;
+    2218       51216 :           border_points_top_transformed.at(i).y = temp_ref.reference.position.y;
+    2219       51216 :           border_points_top_transformed.at(i).z = temp_ref.reference.position.z;
+    2220             : 
+    2221             :         } else {
+    2222           0 :           tf_success = false;
+    2223             :         }
+    2224             :       }
+    2225             : 
+    2226             :       //}
+    2227             : 
+    2228       25608 :       visualization_msgs::Marker safety_area_marker;
+    2229             : 
+    2230       12804 :       safety_area_marker.header.frame_id = _uav_name_ + "/local_origin";
+    2231       12804 :       safety_area_marker.type            = visualization_msgs::Marker::LINE_LIST;
+    2232       12804 :       safety_area_marker.color.a         = 0.15;
+    2233       12804 :       safety_area_marker.scale.x         = 0.2;
+    2234       12804 :       safety_area_marker.color.r         = 1;
+    2235       12804 :       safety_area_marker.color.g         = 0;
+    2236       12804 :       safety_area_marker.color.b         = 0;
+    2237             : 
+    2238       12804 :       safety_area_marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2239             : 
+    2240       25608 :       visualization_msgs::Marker safety_area_coordinates_marker;
+    2241             : 
+    2242       12804 :       safety_area_coordinates_marker.header.frame_id = _uav_name_ + "/local_origin";
+    2243       12804 :       safety_area_coordinates_marker.type            = visualization_msgs::Marker::TEXT_VIEW_FACING;
+    2244       12804 :       safety_area_coordinates_marker.color.a         = 1;
+    2245       12804 :       safety_area_coordinates_marker.scale.z         = 1.0;
+    2246       12804 :       safety_area_coordinates_marker.color.r         = 0;
+    2247       12804 :       safety_area_coordinates_marker.color.g         = 0;
+    2248       12804 :       safety_area_coordinates_marker.color.b         = 0;
+    2249             : 
+    2250       12804 :       safety_area_coordinates_marker.id = 0;
+    2251             : 
+    2252       12804 :       safety_area_coordinates_marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2253             : 
+    2254             :       /* adding safety area points //{ */
+    2255             : 
+    2256             :       // bottom border
+    2257       64020 :       for (size_t i = 0; i < border_points_bot_transformed.size(); i++) {
+    2258             : 
+    2259       51216 :         safety_area_marker.points.push_back(border_points_bot_transformed.at(i));
+    2260       51216 :         safety_area_marker.points.push_back(border_points_bot_transformed.at((i + 1) % border_points_bot_transformed.size()));
+    2261             : 
+    2262      102432 :         std::stringstream ss;
+    2263             : 
+    2264       51216 :         if (_safety_area_horizontal_frame_ == "latlon_origin") {
+    2265           0 :           ss << "idx: " << i << std::endl
+    2266           0 :              << std::setprecision(6) << std::fixed << "lat: " << border_points_bot_original.at(i).x << std::endl
+    2267           0 :              << "lon: " << border_points_bot_original.at(i).y;
+    2268             :         } else {
+    2269       51216 :           ss << "idx: " << i << std::endl
+    2270       51216 :              << std::setprecision(1) << std::fixed << "x: " << border_points_bot_original.at(i).x << std::endl
+    2271       51216 :              << "y: " << border_points_bot_original.at(i).y;
+    2272             :         }
+    2273             : 
+    2274       51216 :         safety_area_coordinates_marker.color.r = 0;
+    2275       51216 :         safety_area_coordinates_marker.color.g = 0;
+    2276       51216 :         safety_area_coordinates_marker.color.b = 0;
+    2277             : 
+    2278       51216 :         safety_area_coordinates_marker.pose.position = border_points_bot_transformed.at(i);
+    2279       51216 :         safety_area_coordinates_marker.text          = ss.str();
+    2280       51216 :         safety_area_coordinates_marker.id++;
+    2281             : 
+    2282       51216 :         safety_area_coordinates_marker_array.markers.push_back(safety_area_coordinates_marker);
+    2283             :       }
+    2284             : 
+    2285             :       // top border + top/bot edges
+    2286       64020 :       for (size_t i = 0; i < border_points_top_transformed.size(); i++) {
+    2287             : 
+    2288       51216 :         safety_area_marker.points.push_back(border_points_top_transformed.at(i));
+    2289       51216 :         safety_area_marker.points.push_back(border_points_top_transformed.at((i + 1) % border_points_top_transformed.size()));
+    2290             : 
+    2291       51216 :         safety_area_marker.points.push_back(border_points_bot_transformed.at(i));
+    2292       51216 :         safety_area_marker.points.push_back(border_points_top_transformed.at(i));
+    2293             : 
+    2294      102432 :         std::stringstream ss;
+    2295             : 
+    2296       51216 :         if (_safety_area_horizontal_frame_ == "latlon_origin") {
+    2297           0 :           ss << "idx: " << i << std::endl
+    2298           0 :              << std::setprecision(6) << std::fixed << "lat: " << border_points_bot_original.at(i).x << std::endl
+    2299           0 :              << "lon: " << border_points_bot_original.at(i).y;
+    2300             :         } else {
+    2301       51216 :           ss << "idx: " << i << std::endl
+    2302       51216 :              << std::setprecision(1) << std::fixed << "x: " << border_points_bot_original.at(i).x << std::endl
+    2303       51216 :              << "y: " << border_points_bot_original.at(i).y;
+    2304             :         }
+    2305             : 
+    2306       51216 :         safety_area_coordinates_marker.color.r = 1;
+    2307       51216 :         safety_area_coordinates_marker.color.g = 1;
+    2308       51216 :         safety_area_coordinates_marker.color.b = 1;
+    2309             : 
+    2310       51216 :         safety_area_coordinates_marker.pose.position = border_points_top_transformed.at(i);
+    2311       51216 :         safety_area_coordinates_marker.text          = ss.str();
+    2312       51216 :         safety_area_coordinates_marker.id++;
+    2313             : 
+    2314       51216 :         safety_area_coordinates_marker_array.markers.push_back(safety_area_coordinates_marker);
+    2315             :       }
+    2316             : 
+    2317             :       //}
+    2318             : 
+    2319       12804 :       if (tf_success) {
+    2320             : 
+    2321       12804 :         safety_area_marker_array.markers.push_back(safety_area_marker);
+    2322             : 
+    2323       12804 :         ph_safety_area_markers_.publish(safety_area_marker_array);
+    2324             : 
+    2325       12804 :         ph_safety_area_coordinates_markers_.publish(safety_area_coordinates_marker_array);
+    2326             :       }
+    2327             : 
+    2328             :     } else {
+    2329        2768 :       ROS_WARN_ONCE("[ControlManager]: missing TFs, can not publish safety area markers");
+    2330             :     }
+    2331             :   }
+    2332             : 
+    2333             :   // --------------------------------------------------------------
+    2334             :   // |              publish the disturbances markers              |
+    2335             :   // --------------------------------------------------------------
+    2336             : 
+    2337       19436 :   if (last_control_output.diagnostics.disturbance_estimator && got_uav_state_) {
+    2338             : 
+    2339       21414 :     visualization_msgs::MarkerArray msg_out;
+    2340             : 
+    2341       10707 :     double id = 0;
+    2342             : 
+    2343       10707 :     double multiplier = 1.0;
+    2344             : 
+    2345       10707 :     Eigen::Quaterniond quat_eigen = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+    2346             : 
+    2347       10707 :     Eigen::Vector3d      vec3d;
+    2348       10707 :     geometry_msgs::Point point;
+    2349             : 
+    2350             :     /* world disturbance //{ */
+    2351             :     {
+    2352             : 
+    2353       21414 :       visualization_msgs::Marker marker;
+    2354             : 
+    2355       10707 :       marker.header.frame_id = uav_state.header.frame_id;
+    2356       10707 :       marker.header.stamp    = ros::Time::now();
+    2357       10707 :       marker.ns              = "control_manager";
+    2358       10707 :       marker.id              = id++;
+    2359       10707 :       marker.type            = visualization_msgs::Marker::ARROW;
+    2360       10707 :       marker.action          = visualization_msgs::Marker::ADD;
+    2361             : 
+    2362             :       /* position //{ */
+    2363             : 
+    2364       10707 :       marker.pose.position.x = 0.0;
+    2365       10707 :       marker.pose.position.y = 0.0;
+    2366       10707 :       marker.pose.position.z = 0.0;
+    2367             : 
+    2368             :       //}
+    2369             : 
+    2370             :       /* orientation //{ */
+    2371             : 
+    2372       10707 :       marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2373             : 
+    2374             :       //}
+    2375             : 
+    2376             :       /* origin //{ */
+    2377       10707 :       point.x = uav_x;
+    2378       10707 :       point.y = uav_y;
+    2379       10707 :       point.z = uav_z;
+    2380             : 
+    2381       10707 :       marker.points.push_back(point);
+    2382             : 
+    2383             :       //}
+    2384             : 
+    2385             :       /* tip //{ */
+    2386             : 
+    2387       10707 :       point.x = uav_x + multiplier * last_control_output.diagnostics.disturbance_wx_w;
+    2388       10707 :       point.y = uav_y + multiplier * last_control_output.diagnostics.disturbance_wy_w;
+    2389       10707 :       point.z = uav_z;
+    2390             : 
+    2391       10707 :       marker.points.push_back(point);
+    2392             : 
+    2393             :       //}
+    2394             : 
+    2395       10707 :       marker.scale.x = 0.05;
+    2396       10707 :       marker.scale.y = 0.05;
+    2397       10707 :       marker.scale.z = 0.05;
+    2398             : 
+    2399       10707 :       marker.color.a = 0.5;
+    2400       10707 :       marker.color.r = 1.0;
+    2401       10707 :       marker.color.g = 0.0;
+    2402       10707 :       marker.color.b = 0.0;
+    2403             : 
+    2404       10707 :       marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+    2405             : 
+    2406       10707 :       msg_out.markers.push_back(marker);
+    2407             :     }
+    2408             : 
+    2409             :     //}
+    2410             : 
+    2411             :     /* body disturbance //{ */
+    2412             :     {
+    2413             : 
+    2414       21414 :       visualization_msgs::Marker marker;
+    2415             : 
+    2416       10707 :       marker.header.frame_id = uav_state.header.frame_id;
+    2417       10707 :       marker.header.stamp    = ros::Time::now();
+    2418       10707 :       marker.ns              = "control_manager";
+    2419       10707 :       marker.id              = id++;
+    2420       10707 :       marker.type            = visualization_msgs::Marker::ARROW;
+    2421       10707 :       marker.action          = visualization_msgs::Marker::ADD;
+    2422             : 
+    2423             :       /* position //{ */
+    2424             : 
+    2425       10707 :       marker.pose.position.x = 0.0;
+    2426       10707 :       marker.pose.position.y = 0.0;
+    2427       10707 :       marker.pose.position.z = 0.0;
+    2428             : 
+    2429             :       //}
+    2430             : 
+    2431             :       /* orientation //{ */
+    2432             : 
+    2433       10707 :       marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2434             : 
+    2435             :       //}
+    2436             : 
+    2437             :       /* origin //{ */
+    2438             : 
+    2439       10707 :       point.x = uav_x;
+    2440       10707 :       point.y = uav_y;
+    2441       10707 :       point.z = uav_z;
+    2442             : 
+    2443       10707 :       marker.points.push_back(point);
+    2444             : 
+    2445             :       //}
+    2446             : 
+    2447             :       /* tip //{ */
+    2448             : 
+    2449       10707 :       vec3d << multiplier * last_control_output.diagnostics.disturbance_bx_b, multiplier * last_control_output.diagnostics.disturbance_by_b, 0;
+    2450       10707 :       vec3d = quat_eigen * vec3d;
+    2451             : 
+    2452       10707 :       point.x = uav_x + vec3d(0);
+    2453       10707 :       point.y = uav_y + vec3d(1);
+    2454       10707 :       point.z = uav_z + vec3d(2);
+    2455             : 
+    2456       10707 :       marker.points.push_back(point);
+    2457             : 
+    2458             :       //}
+    2459             : 
+    2460       10707 :       marker.scale.x = 0.05;
+    2461       10707 :       marker.scale.y = 0.05;
+    2462       10707 :       marker.scale.z = 0.05;
+    2463             : 
+    2464       10707 :       marker.color.a = 0.5;
+    2465       10707 :       marker.color.r = 0.0;
+    2466       10707 :       marker.color.g = 1.0;
+    2467       10707 :       marker.color.b = 0.0;
+    2468             : 
+    2469       10707 :       marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+    2470             : 
+    2471       10707 :       msg_out.markers.push_back(marker);
+    2472             :     }
+    2473             : 
+    2474             :     //}
+    2475             : 
+    2476       10707 :     ph_disturbances_markers_.publish(msg_out);
+    2477             :   }
+    2478             : 
+    2479             :   // --------------------------------------------------------------
+    2480             :   // |               publish the current constraints              |
+    2481             :   // --------------------------------------------------------------
+    2482             : 
+    2483       19436 :   if (got_constraints_) {
+    2484             : 
+    2485       16246 :     auto sanitized_constraints = mrs_lib::get_mutexed(mutex_constraints_, sanitized_constraints_);
+    2486             : 
+    2487       16246 :     mrs_msgs::DynamicsConstraints constraints = sanitized_constraints.constraints;
+    2488             : 
+    2489       16246 :     ph_current_constraints_.publish(constraints);
+    2490             :   }
+    2491             : }
+    2492             : 
+    2493             : //}
+    2494             : 
+    2495             : /* //{ timerSafety() */
+    2496             : 
+    2497      351996 : void ControlManager::timerSafety(const ros::TimerEvent& event) {
+    2498             : 
+    2499      351996 :   mrs_lib::AtomicScopeFlag unset_running(running_safety_timer_);
+    2500             : 
+    2501      351996 :   if (!is_initialized_) {
+    2502           0 :     return;
+    2503             :   }
+    2504             : 
+    2505      703992 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerSafety", _safety_timer_rate_, 0.05, event);
+    2506      703992 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerSafety", scope_timer_logger_, scope_timer_enabled_);
+    2507             : 
+    2508             :   // copy member variables
+    2509      351996 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    2510      351996 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    2511      351996 :   auto [uav_state, uav_yaw]  = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_, uav_yaw_);
+    2512      351996 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    2513      351996 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    2514             : 
+    2515      669691 :   if (!got_uav_state_ || (_state_input_ == INPUT_UAV_STATE && _odometry_innovation_check_enabled_ && !sh_odometry_innovation_.hasMsg()) ||
+    2516      317695 :       active_tracker_idx == _null_tracker_idx_) {
+    2517      119143 :     return;
+    2518             :   }
+    2519             : 
+    2520      232853 :   if (odometry_switch_in_progress_) {
+    2521           5 :     ROS_WARN("[ControlManager]: timerSafety tried to run while odometry switch in progress");
+    2522           5 :     return;
+    2523             :   }
+    2524             : 
+    2525             :   // | ------------------------ timeouts ------------------------ |
+    2526             : 
+    2527      232848 :   if (_state_input_ == INPUT_UAV_STATE && sh_uav_state_.hasMsg()) {
+    2528      232848 :     double missing_for = (ros::Time::now() - sh_uav_state_.lastMsgTime()).toSec();
+    2529             : 
+    2530      232848 :     if (missing_for > _uav_state_max_missing_time_) {
+    2531           0 :       timeoutUavState(missing_for);
+    2532             :     }
+    2533             :   }
+    2534             : 
+    2535      232848 :   if (_state_input_ == INPUT_ODOMETRY && sh_odometry_.hasMsg()) {
+    2536           0 :     double missing_for = (ros::Time::now() - sh_odometry_.lastMsgTime()).toSec();
+    2537             : 
+    2538           0 :     if (missing_for > _uav_state_max_missing_time_) {
+    2539           0 :       timeoutUavState(missing_for);
+    2540             :     }
+    2541             :   }
+    2542             : 
+    2543             :   // | -------------- eland and failsafe thresholds ------------- |
+    2544             : 
+    2545      232848 :   std::map<std::string, ControllerParams>::iterator it;
+    2546      232848 :   it = controllers_.find(_controller_names_.at(active_controller_idx));
+    2547             : 
+    2548      232847 :   _eland_threshold_               = it->second.eland_threshold;
+    2549      232848 :   _failsafe_threshold_            = it->second.failsafe_threshold;
+    2550      232847 :   _odometry_innovation_threshold_ = it->second.odometry_innovation_threshold;
+    2551             : 
+    2552             :   // | --------- calculate control errors and tilt angle -------- |
+    2553             : 
+    2554             :   // This means that the timerFailsafe only does its work when Controllers and Trackers produce valid output.
+    2555             :   // Cases when the commands are not valid should be handle in updateControllers() and updateTrackers() methods.
+    2556      232847 :   if (!last_tracker_cmd || !last_control_output.control_output) {
+    2557         211 :     return;
+    2558             :   }
+    2559             : 
+    2560             :   {
+    2561      232637 :     std::scoped_lock lock(mutex_attitude_error_);
+    2562             : 
+    2563      232637 :     tilt_error_ = 0;
+    2564      232637 :     yaw_error_  = 0;
+    2565             :   }
+    2566             : 
+    2567             :   {
+    2568      232637 :     Eigen::Vector3d position_error = Eigen::Vector3d::Zero();
+    2569             : 
+    2570      232637 :     bool position_error_set = false;
+    2571             : 
+    2572      232637 :     if (last_tracker_cmd->use_position_horizontal && !std::holds_alternative<mrs_msgs::HwApiPositionCmd>(last_control_output.control_output.value())) {
+    2573             : 
+    2574      231587 :       position_error(0) = last_tracker_cmd->position.x - uav_state.pose.position.x;
+    2575      231586 :       position_error(1) = last_tracker_cmd->position.y - uav_state.pose.position.y;
+    2576             : 
+    2577      231586 :       position_error_set = true;
+    2578             :     }
+    2579             : 
+    2580      232636 :     if (last_tracker_cmd->use_position_vertical && !std::holds_alternative<mrs_msgs::HwApiPositionCmd>(last_control_output.control_output.value())) {
+    2581             : 
+    2582      231587 :       position_error(2) = last_tracker_cmd->position.z - uav_state.pose.position.z;
+    2583             : 
+    2584      231587 :       position_error_set = true;
+    2585             :     }
+    2586             : 
+    2587      232637 :     if (position_error_set) {
+    2588             : 
+    2589      231587 :       mrs_lib::set_mutexed(mutex_position_error_, {position_error}, position_error_);
+    2590             :     }
+    2591             :   }
+    2592             : 
+    2593             :   // rotate the drone's z axis
+    2594      232637 :   tf2::Transform uav_state_transform = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+    2595      232637 :   tf2::Vector3   uav_z_in_world      = uav_state_transform * tf2::Vector3(0, 0, 1);
+    2596             : 
+    2597             :   // calculate the angle between the drone's z axis and the world's z axis
+    2598      232637 :   double tilt_angle = acos(uav_z_in_world.dot(tf2::Vector3(0, 0, 1)));
+    2599             : 
+    2600             :   // | ------------ calculate the tilt and yaw error ------------ |
+    2601             : 
+    2602             :   // | --------------------- the tilt error --------------------- |
+    2603             : 
+    2604      232637 :   if (last_control_output.desired_orientation) {
+    2605             : 
+    2606             :     // calculate the desired drone's z axis in the world frame
+    2607      206283 :     tf2::Transform attitude_cmd_transform = mrs_lib::AttitudeConverter(last_control_output.desired_orientation.value());
+    2608      206283 :     tf2::Vector3   uav_z_in_world_desired = attitude_cmd_transform * tf2::Vector3(0, 0, 1);
+    2609             : 
+    2610             :     {
+    2611      206283 :       std::scoped_lock lock(mutex_attitude_error_);
+    2612             : 
+    2613             :       // calculate the angle between the drone's z axis and the world's z axis
+    2614      206283 :       tilt_error_ = acos(uav_z_in_world.dot(uav_z_in_world_desired));
+    2615             : 
+    2616             :       // calculate the yaw error
+    2617      206283 :       double cmd_yaw = mrs_lib::AttitudeConverter(last_control_output.desired_orientation.value()).getYaw();
+    2618      206283 :       yaw_error_     = fabs(radians::diff(cmd_yaw, uav_yaw));
+    2619             :     }
+    2620             :   }
+    2621             : 
+    2622      232637 :   auto position_error          = mrs_lib::get_mutexed(mutex_position_error_, position_error_);
+    2623      232637 :   auto [tilt_error, yaw_error] = mrs_lib::get_mutexed(mutex_attitude_error_, tilt_error_, yaw_error_);
+    2624             : 
+    2625             :   // --------------------------------------------------------------
+    2626             :   // |   activate the failsafe controller in case of large error  |
+    2627             :   // --------------------------------------------------------------
+    2628             : 
+    2629      232637 :   if (position_error) {
+    2630             : 
+    2631      231587 :     if (position_error->norm() > _failsafe_threshold_ && !failsafe_triggered_) {
+    2632             : 
+    2633          14 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2634             : 
+    2635          14 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2636             : 
+    2637           1 :         if (!failsafe_triggered_) {
+    2638             : 
+    2639           1 :           ROS_ERROR("[ControlManager]: activating failsafe land: control_error=%.2f/%.2f m (x: %.2f, y: %.2f, z: %.2f)", position_error->norm(),
+    2640             :                     _failsafe_threshold_, position_error.value()(0), position_error.value()(1), position_error.value()(2));
+    2641             : 
+    2642           1 :           failsafe();
+    2643             :         }
+    2644             :       }
+    2645             :     }
+    2646             :   }
+    2647             : 
+    2648             :   // --------------------------------------------------------------
+    2649             :   // |     activate emergency land in case of large innovation    |
+    2650             :   // --------------------------------------------------------------
+    2651             : 
+    2652      232637 :   if (_odometry_innovation_check_enabled_) {
+    2653             : 
+    2654      232636 :     std::optional<nav_msgs::Odometry::ConstPtr> innovation;
+    2655             : 
+    2656      232637 :     if (sh_odometry_innovation_.hasMsg()) {
+    2657      232637 :       innovation = {sh_odometry_innovation_.getMsg()};
+    2658             :     } else {
+    2659           0 :       ROS_WARN_THROTTLE(1.0, "[ControlManager]: missing estimator innnovation but the innovation check is enabled!");
+    2660             :     }
+    2661             : 
+    2662      232637 :     if (innovation) {
+    2663             : 
+    2664      232637 :       auto [x, y, z] = mrs_lib::getPosition(innovation.value());
+    2665             : 
+    2666      232636 :       double heading = 0;
+    2667             :       try {
+    2668      232636 :         heading = mrs_lib::getHeading(innovation.value());
+    2669             :       }
+    2670           0 :       catch (mrs_lib::AttitudeConverter::GetHeadingException& e) {
+    2671           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception caught: '%s'", e.what());
+    2672             :       }
+    2673             : 
+    2674      232637 :       double last_innovation = mrs_lib::geometry::dist(vec3_t(x, y, z), vec3_t(0, 0, 0));
+    2675             : 
+    2676      232636 :       if (last_innovation > _odometry_innovation_threshold_ || radians::diff(heading, 0) > M_PI_2) {
+    2677             : 
+    2678           1 :         auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2679             : 
+    2680           1 :         if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2681             : 
+    2682           1 :           if (!failsafe_triggered_ && !eland_triggered_) {
+    2683             : 
+    2684           1 :             ROS_ERROR("[ControlManager]: activating emergency land: odometry innovation too large: %.2f/%.2f (x: %.2f, y: %.2f, z: %.2f, heading: %.2f)",
+    2685             :                       last_innovation, _odometry_innovation_threshold_, x, y, z, heading);
+    2686             : 
+    2687           1 :             eland();
+    2688             :           }
+    2689             :         }
+    2690             :       }
+    2691             :     }
+    2692             :   }
+    2693             : 
+    2694             :   // --------------------------------------------------------------
+    2695             :   // |   activate emergency land in case of medium control error  |
+    2696             :   // --------------------------------------------------------------
+    2697             : 
+    2698             :   // | ------------------- tilt control error ------------------- |
+    2699             : 
+    2700      232637 :   if (_tilt_limit_eland_enabled_ && tilt_angle > _tilt_limit_eland_) {
+    2701             : 
+    2702           0 :     auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2703             : 
+    2704           0 :     if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2705             : 
+    2706           0 :       if (!failsafe_triggered_ && !eland_triggered_) {
+    2707             : 
+    2708           0 :         ROS_ERROR("[ControlManager]: activating emergency land: tilt angle too large (%.2f/%.2f deg)", (180.0 / M_PI) * tilt_angle,
+    2709             :                   (180.0 / M_PI) * _tilt_limit_eland_);
+    2710             : 
+    2711           0 :         eland();
+    2712             :       }
+    2713             :     }
+    2714             :   }
+    2715             : 
+    2716             :   // | ----------------- position control error ----------------- |
+    2717             : 
+    2718      232637 :   if (position_error) {
+    2719             : 
+    2720      231585 :     double error_size = position_error->norm();
+    2721             : 
+    2722      231585 :     if (error_size > _eland_threshold_ / 2.0) {
+    2723             : 
+    2724         769 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2725             : 
+    2726         769 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2727             : 
+    2728         538 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2729             : 
+    2730          26 :           ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: releasing payload due to large position error");
+    2731             : 
+    2732          26 :           ungripSrv();
+    2733             :         }
+    2734             :       }
+    2735             :     }
+    2736             : 
+    2737      231585 :     if (error_size > _eland_threshold_) {
+    2738             : 
+    2739         336 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2740             : 
+    2741         336 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2742             : 
+    2743         151 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2744             : 
+    2745           3 :           ROS_ERROR("[ControlManager]: activating emergency land: position error %.2f/%.2f m (error x: %.2f, y: %.2f, z: %.2f)", error_size, _eland_threshold_,
+    2746             :                     position_error.value()(0), position_error.value()(1), position_error.value()(2));
+    2747             : 
+    2748           3 :           eland();
+    2749             :         }
+    2750             :       }
+    2751             :     }
+    2752             :   }
+    2753             : 
+    2754             :   // | -------------------- yaw control error ------------------- |
+    2755             :   // do not have to mutex the yaw_error_ here since I am filling it in this function
+    2756             : 
+    2757      232636 :   if (_yaw_error_eland_enabled_ && yaw_error) {
+    2758             : 
+    2759      232636 :     if (yaw_error.value() > (_yaw_error_eland_ / 2.0)) {
+    2760             : 
+    2761           0 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2762             : 
+    2763           0 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2764             : 
+    2765           0 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2766             : 
+    2767           0 :           ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: releasing payload: yaw error %.2f/%.2f deg", (180.0 / M_PI) * yaw_error.value(),
+    2768             :                              (180.0 / M_PI) * _yaw_error_eland_ / 2.0);
+    2769             : 
+    2770           0 :           ungripSrv();
+    2771             :         }
+    2772             :       }
+    2773             :     }
+    2774             : 
+    2775      232636 :     if (yaw_error.value() > _yaw_error_eland_) {
+    2776             : 
+    2777           0 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2778             : 
+    2779           0 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2780             : 
+    2781           0 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2782             : 
+    2783           0 :           ROS_ERROR("[ControlManager]: activating emergency land: yaw error %.2f/%.2f deg", (180.0 / M_PI) * yaw_error.value(),
+    2784             :                     (180.0 / M_PI) * _yaw_error_eland_);
+    2785             : 
+    2786           0 :           eland();
+    2787             :         }
+    2788             :       }
+    2789             :     }
+    2790             :   }
+    2791             : 
+    2792             :   // --------------------------------------------------------------
+    2793             :   // |      disarm the drone when the tilt exceeds the limit      |
+    2794             :   // --------------------------------------------------------------
+    2795      232636 :   if (_tilt_limit_disarm_enabled_ && tilt_angle > _tilt_limit_disarm_) {
+    2796             : 
+    2797           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_);
+    2798             : 
+    2799           0 :     arming(false);
+    2800             :   }
+    2801             : 
+    2802             :   // --------------------------------------------------------------
+    2803             :   // |     disarm the drone when tilt error exceeds the limit     |
+    2804             :   // --------------------------------------------------------------
+    2805             : 
+    2806      232636 :   if (_tilt_error_disarm_enabled_ && tilt_error) {
+    2807             : 
+    2808      232635 :     auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2809             : 
+    2810             :     // the time from the last controller/tracker switch
+    2811             :     // fyi: we should not
+    2812      232637 :     double time_from_ctrl_tracker_switch = (ros::Time::now() - controller_tracker_switch_time).toSec();
+    2813             : 
+    2814             :     // if the tile error is over the threshold
+    2815             :     // && we are not ramping up during takeoff
+    2816      232637 :     if (fabs(tilt_error.value()) > _tilt_error_disarm_threshold_ && !last_control_output.diagnostics.ramping_up) {
+    2817             : 
+    2818             :       // only account for the error if some time passed from the last tracker/controller switch
+    2819           0 :       if (time_from_ctrl_tracker_switch > 1.0) {
+    2820             : 
+    2821             :         // if the threshold was not exceeded before
+    2822           0 :         if (!tilt_error_disarm_over_thr_) {
+    2823             : 
+    2824           0 :           tilt_error_disarm_over_thr_ = true;
+    2825           0 :           tilt_error_disarm_time_     = ros::Time::now();
+    2826             : 
+    2827           0 :           ROS_WARN("[ControlManager]: tilt error exceeded threshold (%.2f/%.2f deg)", (180.0 / M_PI) * tilt_error.value(),
+    2828             :                    (180.0 / M_PI) * _tilt_error_disarm_threshold_);
+    2829             : 
+    2830             :           // if it was exceeded before, just keep it
+    2831             :         } else {
+    2832             : 
+    2833           0 :           ROS_WARN_THROTTLE(0.1, "[ControlManager]: tilt error (%.2f deg) over threshold for %.2f s", (180.0 / M_PI) * tilt_error.value(),
+    2834             :                             (ros::Time::now() - tilt_error_disarm_time_).toSec());
+    2835             :         }
+    2836             : 
+    2837             :         // if the tile error is bad, but the controller just switched,
+    2838             :         // don't think its bad anymore
+    2839             :       } else {
+    2840             : 
+    2841           0 :         tilt_error_disarm_over_thr_ = false;
+    2842           0 :         tilt_error_disarm_time_     = ros::Time::now();
+    2843             :       }
+    2844             : 
+    2845             :       // if the tilt error is fine
+    2846             :     } else {
+    2847             : 
+    2848             :       // make it fine
+    2849      232637 :       tilt_error_disarm_over_thr_ = false;
+    2850      232637 :       tilt_error_disarm_time_     = ros::Time::now();
+    2851             :     }
+    2852             : 
+    2853             :     // calculate the time over the threshold
+    2854      232637 :     double tot = (ros::Time::now() - tilt_error_disarm_time_).toSec();
+    2855             : 
+    2856             :     // if the tot exceeds the limit (and if we are actually over the threshold)
+    2857      232637 :     if (tilt_error_disarm_over_thr_ && (tot > _tilt_error_disarm_timeout_)) {
+    2858             : 
+    2859           0 :       bool is_flying = offboard_mode_ && active_tracker_idx != _null_tracker_idx_;
+    2860             : 
+    2861             :       // only when flying and not in failsafe
+    2862           0 :       if (is_flying) {
+    2863             : 
+    2864           0 :         ROS_ERROR("[ControlManager]: tilt error too large for %.2f s, disarming", tot);
+    2865             : 
+    2866           0 :         toggleOutput(false);
+    2867           0 :         arming(false);
+    2868             :       }
+    2869             :     }
+    2870             :   }
+    2871             : 
+    2872             :   // | --------- dropping out of OFFBOARD in mid flight --------- |
+    2873             : 
+    2874             :   // if we are not in offboard and the drone is in mid air (NullTracker is not active)
+    2875      232639 :   if (offboard_mode_was_true_ && !offboard_mode_ && active_tracker_idx != _null_tracker_idx_) {
+    2876             : 
+    2877           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: we fell out of OFFBOARD in mid air, disabling output");
+    2878             : 
+    2879           0 :     toggleOutput(false);
+    2880             :   }
+    2881             : }  // namespace control_manager
+    2882             : 
+    2883             : //}
+    2884             : 
+    2885             : /* //{ timerEland() */
+    2886             : 
+    2887         482 : void ControlManager::timerEland(const ros::TimerEvent& event) {
+    2888             : 
+    2889         482 :   if (!is_initialized_) {
+    2890         115 :     return;
+    2891             :   }
+    2892             : 
+    2893         964 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerEland", _elanding_timer_rate_, 0.01, event);
+    2894         964 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerEland", scope_timer_logger_, scope_timer_enabled_);
+    2895             : 
+    2896             :   // copy member variables
+    2897         482 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    2898             : 
+    2899         482 :   if (!last_control_output.control_output) {
+    2900           0 :     return;
+    2901             :   }
+    2902             : 
+    2903         482 :   auto throttle = extractThrottle(last_control_output);
+    2904             : 
+    2905         482 :   if (!throttle) {
+    2906         115 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: TODO: implement landing detection mechanism for the current control modality");
+    2907         115 :     return;
+    2908             :   }
+    2909             : 
+    2910         367 :   if (current_state_landing_ == IDLE_STATE) {
+    2911             : 
+    2912           0 :     return;
+    2913             : 
+    2914         367 :   } else if (current_state_landing_ == LANDING_STATE) {
+    2915             : 
+    2916             :     // --------------------------------------------------------------
+    2917             :     // |                            TODO                            |
+    2918             :     // This section needs work. The throttle landing detection      |
+    2919             :     // mechanism should be extracted and other mechanisms, such     |
+    2920             :     // as velocity-based detection should be used for high          |
+    2921             :     // modalities                                                   |
+    2922             :     // --------------------------------------------------------------
+    2923             : 
+    2924         367 :     if (!last_control_output.control_output) {
+    2925           0 :       ROS_WARN_THROTTLE(1.0, "[ControlManager]: timerEland: last_control_output has not been initialized, returning");
+    2926           0 :       ROS_WARN_THROTTLE(1.0, "[ControlManager]: tip: the RC eland is probably triggered");
+    2927           0 :       return;
+    2928             :     }
+    2929             : 
+    2930             :     // recalculate the mass based on the throttle
+    2931         367 :     throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value()) / common_handlers_->g;
+    2932         367 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: landing: initial mass: %.2f throttle_mass_estimate: %.2f", landing_uav_mass_, throttle_mass_estimate_);
+    2933             : 
+    2934             :     // condition for automatic motor turn off
+    2935         367 :     if (((throttle_mass_estimate_ < _elanding_cutoff_mass_factor_ * landing_uav_mass_) || throttle < 0.01)) {
+    2936          88 :       if (!throttle_under_threshold_) {
+    2937             : 
+    2938           4 :         throttle_mass_estimate_first_time_ = ros::Time::now();
+    2939           4 :         throttle_under_threshold_          = true;
+    2940             :       }
+    2941             : 
+    2942          88 :       ROS_INFO_THROTTLE(0.1, "[ControlManager]: throttle is under cutoff factor for %.2f s", (ros::Time::now() - throttle_mass_estimate_first_time_).toSec());
+    2943             : 
+    2944             :     } else {
+    2945         279 :       throttle_mass_estimate_first_time_ = ros::Time::now();
+    2946         279 :       throttle_under_threshold_          = false;
+    2947             :     }
+    2948             : 
+    2949         367 :     if (throttle_under_threshold_ && ((ros::Time::now() - throttle_mass_estimate_first_time_).toSec() > _elanding_cutoff_timeout_)) {
+    2950             :       // enable callbacks? ... NO
+    2951             : 
+    2952           4 :       ROS_INFO("[ControlManager]: reached cutoff throttle, disabling output");
+    2953           4 :       toggleOutput(false);
+    2954             : 
+    2955             :       // disarm the drone
+    2956           4 :       if (_eland_disarm_enabled_) {
+    2957             : 
+    2958           4 :         ROS_INFO("[ControlManager]: calling for disarm");
+    2959           4 :         arming(false);
+    2960             :       }
+    2961             : 
+    2962           4 :       changeLandingState(IDLE_STATE);
+    2963             : 
+    2964           4 :       ROS_WARN("[ControlManager]: emergency landing finished");
+    2965             : 
+    2966           4 :       ROS_DEBUG("[ControlManager]: stopping eland timer");
+    2967           4 :       timer_eland_.stop();
+    2968           4 :       ROS_DEBUG("[ControlManager]: eland timer stopped");
+    2969             : 
+    2970             :       // we should NOT set eland_triggered_=true
+    2971             :     }
+    2972             :   }
+    2973             : }
+    2974             : 
+    2975             : //}
+    2976             : 
+    2977             : /* //{ timerFailsafe() */
+    2978             : 
+    2979        9716 : void ControlManager::timerFailsafe(const ros::TimerEvent& event) {
+    2980             : 
+    2981        9716 :   if (!is_initialized_) {
+    2982           0 :     return;
+    2983             :   }
+    2984             : 
+    2985        9716 :   ROS_INFO_ONCE("[ControlManager]: timerFailsafe() spinning");
+    2986             : 
+    2987       19432 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerFailsafe", _failsafe_timer_rate_, 0.01, event);
+    2988       19432 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerFailsafe", scope_timer_logger_, scope_timer_enabled_);
+    2989             : 
+    2990             :   // copy member variables
+    2991        9716 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    2992             : 
+    2993        9716 :   updateControllers(uav_state);
+    2994             : 
+    2995        9716 :   publish();
+    2996             : 
+    2997        9716 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    2998             : 
+    2999        9716 :   if (!last_control_output.control_output) {
+    3000           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: timerFailsafe: the control output produced by the failsafe controller is empty!");
+    3001           0 :     return;
+    3002             :   }
+    3003             : 
+    3004        9716 :   auto throttle = extractThrottle(last_control_output);
+    3005             : 
+    3006        9716 :   if (!throttle) {
+    3007           0 :     ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: FailsafeTimer: could not extract throttle out of the last control output");
+    3008           0 :     return;
+    3009             :   }
+    3010             : 
+    3011             :   // --------------------------------------------------------------
+    3012             :   // |                            TODO                            |
+    3013             :   // This section needs work. The throttle landing detection      |
+    3014             :   // mechanism should be extracted and other mechanisms, such     |
+    3015             :   // as velocity-based detection should be used for high          |
+    3016             :   // modalities                                                   |
+    3017             :   // --------------------------------------------------------------
+    3018             : 
+    3019        9716 :   double throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value()) / common_handlers_->g;
+    3020        9716 :   ROS_INFO_THROTTLE(1.0, "[ControlManager]: failsafe: initial mass: %.2f throttle_mass_estimate: %.2f", landing_uav_mass_, throttle_mass_estimate_);
+    3021             : 
+    3022             :   // condition for automatic motor turn off
+    3023        9716 :   if (((throttle_mass_estimate_ < _elanding_cutoff_mass_factor_ * landing_uav_mass_))) {
+    3024             : 
+    3025        1414 :     if (!throttle_under_threshold_) {
+    3026             : 
+    3027           7 :       throttle_mass_estimate_first_time_ = ros::Time::now();
+    3028           7 :       throttle_under_threshold_          = true;
+    3029             :     }
+    3030             : 
+    3031        1414 :     ROS_INFO_THROTTLE(0.1, "[ControlManager]: throttle is under cutoff factor for %.2f s", (ros::Time::now() - throttle_mass_estimate_first_time_).toSec());
+    3032             : 
+    3033             :   } else {
+    3034             : 
+    3035        8302 :     throttle_mass_estimate_first_time_ = ros::Time::now();
+    3036        8302 :     throttle_under_threshold_          = false;
+    3037             :   }
+    3038             : 
+    3039             :   // condition for automatic motor turn off
+    3040        9716 :   if (throttle_under_threshold_ && ((ros::Time::now() - throttle_mass_estimate_first_time_).toSec() > _elanding_cutoff_timeout_)) {
+    3041             : 
+    3042           7 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: detecting zero throttle, disarming");
+    3043             : 
+    3044           7 :     arming(false);
+    3045             :   }
+    3046             : }
+    3047             : 
+    3048             : //}
+    3049             : 
+    3050             : /* //{ timerJoystick() */
+    3051             : 
+    3052       68187 : void ControlManager::timerJoystick(const ros::TimerEvent& event) {
+    3053             : 
+    3054       68187 :   if (!is_initialized_) {
+    3055           0 :     return;
+    3056             :   }
+    3057             : 
+    3058      204561 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerJoystick", _status_timer_rate_, 0.05, event);
+    3059      204561 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerJoystick", scope_timer_logger_, scope_timer_enabled_);
+    3060             : 
+    3061      136374 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    3062             : 
+    3063             :   // if start was pressed and held for > 3.0 s
+    3064       68187 :   if (joystick_start_pressed_ && joystick_start_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_start_press_time_).toSec() > 3.0) {
+    3065             : 
+    3066           0 :     joystick_start_press_time_ = ros::Time(0);
+    3067             : 
+    3068           0 :     ROS_INFO("[ControlManager]: transitioning to joystick control: activating '%s' and '%s'", _joystick_tracker_name_.c_str(),
+    3069             :              _joystick_controller_name_.c_str());
+    3070             : 
+    3071           0 :     joystick_start_pressed_ = false;
+    3072             : 
+    3073           0 :     switchTracker(_joystick_tracker_name_);
+    3074           0 :     switchController(_joystick_controller_name_);
+    3075             :   }
+    3076             : 
+    3077             :   // if RT+LT were pressed and held for > 0.1 s
+    3078       68187 :   if (joystick_failsafe_pressed_ && joystick_failsafe_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_failsafe_press_time_).toSec() > 0.1) {
+    3079             : 
+    3080           0 :     joystick_failsafe_press_time_ = ros::Time(0);
+    3081             : 
+    3082           0 :     ROS_INFO("[ControlManager]: activating failsafe by joystick");
+    3083             : 
+    3084           0 :     joystick_failsafe_pressed_ = false;
+    3085             : 
+    3086           0 :     failsafe();
+    3087             :   }
+    3088             : 
+    3089             :   // if joypads were pressed and held for > 0.1 s
+    3090       68187 :   if (joystick_eland_pressed_ && joystick_eland_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_eland_press_time_).toSec() > 0.1) {
+    3091             : 
+    3092           0 :     joystick_eland_press_time_ = ros::Time(0);
+    3093             : 
+    3094           0 :     ROS_INFO("[ControlManager]: activating eland by joystick");
+    3095             : 
+    3096           0 :     joystick_failsafe_pressed_ = false;
+    3097             : 
+    3098           0 :     eland();
+    3099             :   }
+    3100             : 
+    3101             :   // if back was pressed and held for > 0.1 s
+    3102       68187 :   if (joystick_back_pressed_ && joystick_back_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_back_press_time_).toSec() > 0.1) {
+    3103             : 
+    3104           0 :     joystick_back_press_time_ = ros::Time(0);
+    3105             : 
+    3106             :     // activate/deactivate the joystick goto functionality
+    3107           0 :     joystick_goto_enabled_ = !joystick_goto_enabled_;
+    3108             : 
+    3109           0 :     ROS_INFO("[ControlManager]: joystick control %s", joystick_goto_enabled_ ? "activated" : "deactivated");
+    3110             :   }
+    3111             : 
+    3112             :   // if the GOTO functionality is enabled...
+    3113       68187 :   if (joystick_goto_enabled_ && sh_joystick_.hasMsg()) {
+    3114             : 
+    3115           0 :     auto joystick_data = sh_joystick_.getMsg();
+    3116             : 
+    3117             :     // create the reference
+    3118             : 
+    3119           0 :     mrs_msgs::Vec4::Request request;
+    3120             : 
+    3121           0 :     if (fabs(joystick_data->axes.at(_channel_pitch_)) >= 0.05 || fabs(joystick_data->axes.at(_channel_roll_)) >= 0.05 ||
+    3122           0 :         fabs(joystick_data->axes.at(_channel_heading_)) >= 0.05 || fabs(joystick_data->axes.at(_channel_throttle_)) >= 0.05) {
+    3123             : 
+    3124           0 :       if (_joystick_mode_ == 0) {
+    3125             : 
+    3126           0 :         request.goal.at(REF_X)       = _channel_mult_pitch_ * joystick_data->axes.at(_channel_pitch_) * _joystick_carrot_distance_;
+    3127           0 :         request.goal.at(REF_Y)       = _channel_mult_roll_ * joystick_data->axes.at(_channel_roll_) * _joystick_carrot_distance_;
+    3128           0 :         request.goal.at(REF_Z)       = _channel_mult_throttle_ * joystick_data->axes.at(_channel_throttle_);
+    3129           0 :         request.goal.at(REF_HEADING) = _channel_mult_heading_ * joystick_data->axes.at(_channel_heading_);
+    3130             : 
+    3131           0 :         mrs_msgs::Vec4::Response response;
+    3132             : 
+    3133           0 :         callbackGotoFcu(request, response);
+    3134             : 
+    3135           0 :       } else if (_joystick_mode_ == 1) {
+    3136             : 
+    3137           0 :         mrs_msgs::TrajectoryReference trajectory;
+    3138             : 
+    3139           0 :         double dt = 0.2;
+    3140             : 
+    3141           0 :         trajectory.fly_now         = true;
+    3142           0 :         trajectory.header.frame_id = "fcu_untilted";
+    3143           0 :         trajectory.use_heading     = true;
+    3144           0 :         trajectory.dt              = dt;
+    3145             : 
+    3146           0 :         mrs_msgs::Reference point;
+    3147           0 :         point.position.x = 0;
+    3148           0 :         point.position.y = 0;
+    3149           0 :         point.position.z = 0;
+    3150           0 :         point.heading    = 0;
+    3151             : 
+    3152           0 :         trajectory.points.push_back(point);
+    3153             : 
+    3154           0 :         double speed = 1.0;
+    3155             : 
+    3156           0 :         for (int i = 0; i < 50; i++) {
+    3157             : 
+    3158           0 :           point.position.x += _channel_mult_pitch_ * joystick_data->axes.at(_channel_pitch_) * (speed * dt);
+    3159           0 :           point.position.y += _channel_mult_roll_ * joystick_data->axes.at(_channel_roll_) * (speed * dt);
+    3160           0 :           point.position.z += _channel_mult_throttle_ * joystick_data->axes.at(_channel_throttle_) * (speed * dt);
+    3161           0 :           point.heading = _channel_mult_heading_ * joystick_data->axes.at(_channel_heading_);
+    3162             : 
+    3163           0 :           trajectory.points.push_back(point);
+    3164             :         }
+    3165             : 
+    3166           0 :         setTrajectoryReference(trajectory);
+    3167             :       }
+    3168             :     }
+    3169             :   }
+    3170             : 
+    3171       68187 :   if (rc_goto_active_ && last_tracker_cmd && sh_hw_api_rc_.hasMsg()) {
+    3172             : 
+    3173             :     // create the reference
+    3174        1570 :     mrs_msgs::VelocityReferenceStampedSrv::Request request;
+    3175             : 
+    3176         785 :     double des_x       = 0;
+    3177         785 :     double des_y       = 0;
+    3178         785 :     double des_z       = 0;
+    3179         785 :     double des_heading = 0;
+    3180             : 
+    3181         785 :     bool nothing_to_do = true;
+    3182             : 
+    3183             :     // copy member variables
+    3184        1570 :     mrs_msgs::HwApiRcChannelsConstPtr rc_channels = sh_hw_api_rc_.getMsg();
+    3185             : 
+    3186         785 :     if (rc_channels->channels.size() < 4) {
+    3187             : 
+    3188           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: RC control channel numbers are out of range (the # of channels in rc/in topic is %d)",
+    3189             :                          int(rc_channels->channels.size()));
+    3190           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: tip: this could be caused by the RC failsafe not being configured!");
+    3191             : 
+    3192             :     } else {
+    3193             : 
+    3194         785 :       double tmp_x       = RCChannelToRange(rc_channels->channels.at(_rc_channel_pitch_), _rc_horizontal_speed_, 0.1);
+    3195         785 :       double tmp_y       = -RCChannelToRange(rc_channels->channels.at(_rc_channel_roll_), _rc_horizontal_speed_, 0.1);
+    3196         785 :       double tmp_z       = RCChannelToRange(rc_channels->channels.at(_rc_channel_throttle_), _rc_vertical_speed_, 0.3);
+    3197         785 :       double tmp_heading = -RCChannelToRange(rc_channels->channels.at(_rc_channel_heading_), _rc_heading_rate_, 0.1);
+    3198             : 
+    3199         785 :       if (abs(tmp_x) > 1e-3) {
+    3200         260 :         des_x         = tmp_x;
+    3201         260 :         nothing_to_do = false;
+    3202             :       }
+    3203             : 
+    3204         785 :       if (abs(tmp_y) > 1e-3) {
+    3205         119 :         des_y         = tmp_y;
+    3206         119 :         nothing_to_do = false;
+    3207             :       }
+    3208             : 
+    3209         785 :       if (abs(tmp_z) > 1e-3) {
+    3210         261 :         des_z         = tmp_z;
+    3211         261 :         nothing_to_do = false;
+    3212             :       }
+    3213             : 
+    3214         785 :       if (abs(tmp_heading) > 1e-3) {
+    3215          77 :         des_heading   = tmp_heading;
+    3216          77 :         nothing_to_do = false;
+    3217             :       }
+    3218             :     }
+    3219             : 
+    3220         785 :     if (!nothing_to_do) {
+    3221             : 
+    3222         717 :       request.reference.header.frame_id = "fcu_untilted";
+    3223             : 
+    3224         717 :       request.reference.reference.use_heading_rate = true;
+    3225             : 
+    3226         717 :       request.reference.reference.velocity.x   = des_x;
+    3227         717 :       request.reference.reference.velocity.y   = des_y;
+    3228         717 :       request.reference.reference.velocity.z   = des_z;
+    3229         717 :       request.reference.reference.heading_rate = des_heading;
+    3230             : 
+    3231        1434 :       mrs_msgs::VelocityReferenceStampedSrv::Response response;
+    3232             : 
+    3233             :       // disable callbacks of all trackers
+    3234         717 :       std_srvs::SetBoolRequest req_enable_callbacks;
+    3235             : 
+    3236             :       // enable the callbacks for the active tracker
+    3237         717 :       req_enable_callbacks.data = true;
+    3238             :       {
+    3239         717 :         std::scoped_lock lock(mutex_tracker_list_);
+    3240             : 
+    3241         717 :         tracker_list_.at(active_tracker_idx_)
+    3242         717 :             ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3243             :       }
+    3244             : 
+    3245         717 :       callbacks_enabled_ = true;
+    3246             : 
+    3247         717 :       callbackVelocityReferenceService(request, response);
+    3248             : 
+    3249         717 :       callbacks_enabled_ = false;
+    3250             : 
+    3251         717 :       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);
+    3252             : 
+    3253             :       // disable the callbacks back again
+    3254         717 :       req_enable_callbacks.data = false;
+    3255             :       {
+    3256         717 :         std::scoped_lock lock(mutex_tracker_list_);
+    3257             : 
+    3258         717 :         tracker_list_.at(active_tracker_idx_)
+    3259         717 :             ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3260             :       }
+    3261             :     }
+    3262             :   }
+    3263             : }
+    3264             : 
+    3265             : //}
+    3266             : 
+    3267             : /* //{ timerBumper() */
+    3268             : 
+    3269        2484 : void ControlManager::timerBumper(const ros::TimerEvent& event) {
+    3270             : 
+    3271        2484 :   if (!is_initialized_) {
+    3272        2208 :     return;
+    3273             :   }
+    3274             : 
+    3275        4968 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerBumper", _bumper_timer_rate_, 0.05, event);
+    3276        4968 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerBumper", scope_timer_logger_, scope_timer_enabled_);
+    3277             : 
+    3278             :   // | ---------------------- preconditions --------------------- |
+    3279             : 
+    3280        2484 :   if (!sh_bumper_.hasMsg()) {
+    3281        2205 :     return;
+    3282             :   }
+    3283             : 
+    3284         279 :   if (!bumper_enabled_) {
+    3285           0 :     return;
+    3286             :   }
+    3287             : 
+    3288         279 :   if (!isFlyingNormally() && !bumper_repulsing_) {
+    3289           3 :     return;
+    3290             :   }
+    3291             : 
+    3292         276 :   if (!got_uav_state_) {
+    3293           0 :     return;
+    3294             :   }
+    3295             : 
+    3296         276 :   if (sh_bumper_.hasMsg() && (ros::Time::now() - sh_bumper_.lastMsgTime()).toSec() > 1.0) {
+    3297             : 
+    3298           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: obstacle bumper is missing messages although we got them before");
+    3299             : 
+    3300           0 :     return;
+    3301             :   }
+    3302             : 
+    3303         276 :   timer_bumper_.setPeriod(ros::Duration(1.0 / _bumper_timer_rate_));
+    3304             : 
+    3305             :   // | ------------------ call the bumper logic ----------------- |
+    3306             : 
+    3307         276 :   bumperPushFromObstacle();
+    3308             : }
+    3309             : 
+    3310             : //}
+    3311             : 
+    3312             : /* //{ timerPirouette() */
+    3313             : 
+    3314           0 : void ControlManager::timerPirouette(const ros::TimerEvent& event) {
+    3315             : 
+    3316           0 :   if (!is_initialized_) {
+    3317           0 :     return;
+    3318             :   }
+    3319             : 
+    3320           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerPirouette", _pirouette_timer_rate_, 0.01, event);
+    3321           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerPirouette", scope_timer_logger_, scope_timer_enabled_);
+    3322             : 
+    3323           0 :   pirouette_iterator_++;
+    3324             : 
+    3325           0 :   double pirouette_duration  = (2 * M_PI) / _pirouette_speed_;
+    3326           0 :   double pirouette_n_steps   = pirouette_duration * _pirouette_timer_rate_;
+    3327           0 :   double pirouette_step_size = (2 * M_PI) / pirouette_n_steps;
+    3328             : 
+    3329           0 :   if (rc_escalating_failsafe_triggered_ || failsafe_triggered_ || eland_triggered_ || (pirouette_iterator_ > pirouette_duration * _pirouette_timer_rate_)) {
+    3330             : 
+    3331           0 :     _pirouette_enabled_ = false;
+    3332           0 :     timer_pirouette_.stop();
+    3333             : 
+    3334           0 :     setCallbacks(true);
+    3335             : 
+    3336           0 :     return;
+    3337             :   }
+    3338             : 
+    3339             :   // set the reference
+    3340           0 :   mrs_msgs::ReferenceStamped reference_request;
+    3341             : 
+    3342           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    3343             : 
+    3344           0 :   reference_request.header.frame_id      = "";
+    3345           0 :   reference_request.header.stamp         = ros::Time(0);
+    3346           0 :   reference_request.reference.position.x = last_tracker_cmd->position.x;
+    3347           0 :   reference_request.reference.position.y = last_tracker_cmd->position.y;
+    3348           0 :   reference_request.reference.position.z = last_tracker_cmd->position.z;
+    3349           0 :   reference_request.reference.heading    = pirouette_initial_heading_ + pirouette_iterator_ * pirouette_step_size;
+    3350             : 
+    3351             :   // enable the callbacks for the active tracker
+    3352             :   {
+    3353           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    3354             : 
+    3355           0 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    3356           0 :     req_enable_callbacks.data = true;
+    3357             : 
+    3358           0 :     tracker_list_.at(active_tracker_idx_)
+    3359           0 :         ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3360             : 
+    3361           0 :     callbacks_enabled_ = true;
+    3362             :   }
+    3363             : 
+    3364           0 :   setReference(reference_request);
+    3365             : 
+    3366             :   {
+    3367           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    3368             : 
+    3369             :     // disable the callbacks for the active tracker
+    3370           0 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    3371           0 :     req_enable_callbacks.data = false;
+    3372             : 
+    3373           0 :     tracker_list_.at(active_tracker_idx_)
+    3374           0 :         ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3375             : 
+    3376           0 :     callbacks_enabled_ = false;
+    3377             :   }
+    3378             : }
+    3379             : 
+    3380             : //}
+    3381             : 
+    3382             : // --------------------------------------------------------------
+    3383             : // |                           asyncs                           |
+    3384             : // --------------------------------------------------------------
+    3385             : 
+    3386             : /* asyncControl() //{ */
+    3387             : 
+    3388      151343 : void ControlManager::asyncControl(void) {
+    3389             : 
+    3390      151343 :   if (!is_initialized_) {
+    3391           0 :     return;
+    3392             :   }
+    3393             : 
+    3394      302686 :   mrs_lib::AtomicScopeFlag unset_running(running_async_control_);
+    3395             : 
+    3396      454029 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("asyncControl");
+    3397      454029 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::asyncControl", scope_timer_logger_, scope_timer_enabled_);
+    3398             : 
+    3399             :   // copy member variables
+    3400      302686 :   auto uav_state           = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    3401      151343 :   auto current_constraints = mrs_lib::get_mutexed(mutex_constraints_, current_constraints_);
+    3402             : 
+    3403      151343 :   if (!failsafe_triggered_) {  // when failsafe is triggered, updateControllers() and publish() is called in timerFailsafe()
+    3404             : 
+    3405             :     // run the safety timer
+    3406             :     // in the case of large control errors, the safety mechanisms will be triggered before the controllers and trackers are updated...
+    3407      142115 :     while (running_safety_timer_) {
+    3408             : 
+    3409         808 :       ROS_DEBUG("[ControlManager]: waiting for safety timer to finish");
+    3410         808 :       ros::Duration wait(0.001);
+    3411         808 :       wait.sleep();
+    3412             : 
+    3413         808 :       if (!running_safety_timer_) {
+    3414         802 :         ROS_DEBUG("[ControlManager]: safety timer finished");
+    3415         802 :         break;
+    3416             :       }
+    3417             :     }
+    3418             : 
+    3419      142109 :     ros::TimerEvent safety_timer_event;
+    3420      142109 :     timerSafety(safety_timer_event);
+    3421             : 
+    3422      142109 :     updateTrackers();
+    3423             : 
+    3424      142109 :     updateControllers(uav_state);
+    3425             : 
+    3426      142109 :     if (got_constraints_) {
+    3427             : 
+    3428             :       // update the constraints to trackers, if need to
+    3429      141427 :       auto enforce = enforceControllersConstraints(current_constraints);
+    3430             : 
+    3431      141427 :       if (enforce && !constraints_being_enforced_) {
+    3432             : 
+    3433          86 :         setConstraintsToTrackers(enforce.value());
+    3434          86 :         mrs_lib::set_mutexed(mutex_constraints_, enforce.value(), sanitized_constraints_);
+    3435             : 
+    3436          86 :         constraints_being_enforced_ = true;
+    3437             : 
+    3438          86 :         auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    3439             : 
+    3440          86 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: the controller '%s' is enforcing constraints over the ConstraintManager",
+    3441             :                           _controller_names_.at(active_controller_idx).c_str());
+    3442             : 
+    3443      141341 :       } else if (!enforce && constraints_being_enforced_) {
+    3444             : 
+    3445          80 :         ROS_INFO_THROTTLE(1.0, "[ControlManager]: constraint values returned to original values");
+    3446             : 
+    3447          80 :         constraints_being_enforced_ = false;
+    3448             : 
+    3449          80 :         mrs_lib::set_mutexed(mutex_constraints_, current_constraints, sanitized_constraints_);
+    3450             : 
+    3451          80 :         setConstraintsToTrackers(current_constraints);
+    3452             :       }
+    3453             :     }
+    3454             : 
+    3455      142109 :     publish();
+    3456             :   }
+    3457             : 
+    3458             :   // if odometry switch happened, we finish it here and turn the safety timer back on
+    3459      151343 :   if (odometry_switch_in_progress_) {
+    3460             : 
+    3461           5 :     ROS_DEBUG("[ControlManager]: starting safety timer");
+    3462           5 :     timer_safety_.start();
+    3463           5 :     ROS_DEBUG("[ControlManager]: safety timer started");
+    3464           5 :     odometry_switch_in_progress_ = false;
+    3465             : 
+    3466             :     {
+    3467          10 :       std::scoped_lock lock(mutex_uav_state_);
+    3468             : 
+    3469           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,
+    3470             :                uav_state.pose.position.z, uav_heading_);
+    3471             :     }
+    3472             :   }
+    3473             : }
+    3474             : 
+    3475             : //}
+    3476             : 
+    3477             : // --------------------------------------------------------------
+    3478             : // |                          callbacks                         |
+    3479             : // --------------------------------------------------------------
+    3480             : 
+    3481             : // | --------------------- topic callbacks -------------------- |
+    3482             : 
+    3483             : /* //{ callbackOdometry() */
+    3484             : 
+    3485           0 : void ControlManager::callbackOdometry(const nav_msgs::Odometry::ConstPtr msg) {
+    3486             : 
+    3487           0 :   if (!is_initialized_) {
+    3488           0 :     return;
+    3489             :   }
+    3490             : 
+    3491           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackOdometry");
+    3492           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackOdometry", scope_timer_logger_, scope_timer_enabled_);
+    3493             : 
+    3494           0 :   nav_msgs::OdometryConstPtr odom = msg;
+    3495             : 
+    3496             :   // | ------------------ check for time stamp ------------------ |
+    3497             : 
+    3498             :   {
+    3499           0 :     std::scoped_lock lock(mutex_uav_state_);
+    3500             : 
+    3501           0 :     if (uav_state_.header.stamp == odom->header.stamp) {
+    3502           0 :       return;
+    3503             :     }
+    3504             :   }
+    3505             : 
+    3506             :   // | --------------------- check for nans --------------------- |
+    3507             : 
+    3508           0 :   if (!validateOdometry(*odom, "ControlManager", "odometry")) {
+    3509           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: incoming 'odometry' contains invalid values, throwing it away");
+    3510           0 :     return;
+    3511             :   }
+    3512             : 
+    3513             :   // | ---------------------- frame switch ---------------------- |
+    3514             : 
+    3515             :   /* Odometry frame switch //{ */
+    3516             : 
+    3517             :   // | -- prepare an OdometryConstPtr for trackers & controllers -- |
+    3518             : 
+    3519           0 :   mrs_msgs::UavState uav_state_odom;
+    3520             : 
+    3521           0 :   uav_state_odom.header   = odom->header;
+    3522           0 :   uav_state_odom.pose     = odom->pose.pose;
+    3523           0 :   uav_state_odom.velocity = odom->twist.twist;
+    3524             : 
+    3525             :   // | ----- check for change in odometry frame of reference ---- |
+    3526             : 
+    3527           0 :   if (got_uav_state_) {
+    3528             : 
+    3529           0 :     if (odom->header.frame_id != uav_state_.header.frame_id) {
+    3530             : 
+    3531           0 :       ROS_INFO("[ControlManager]: detecting switch of odometry frame");
+    3532             :       {
+    3533           0 :         std::scoped_lock lock(mutex_uav_state_);
+    3534             : 
+    3535           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,
+    3536             :                  uav_state_.pose.position.z, uav_heading_);
+    3537             :       }
+    3538             : 
+    3539           0 :       odometry_switch_in_progress_ = true;
+    3540             : 
+    3541             :       // we have to stop safety timer, otherwise it will interfere
+    3542           0 :       ROS_DEBUG("[ControlManager]: stopping the safety timer");
+    3543           0 :       timer_safety_.stop();
+    3544           0 :       ROS_DEBUG("[ControlManager]: safety timer stopped");
+    3545             : 
+    3546             :       // wait for the safety timer to stop if its running
+    3547           0 :       while (running_safety_timer_) {
+    3548             : 
+    3549           0 :         ROS_DEBUG("[ControlManager]: waiting for safety timer to finish");
+    3550           0 :         ros::Duration wait(0.001);
+    3551           0 :         wait.sleep();
+    3552             : 
+    3553           0 :         if (!running_safety_timer_) {
+    3554           0 :           ROS_DEBUG("[ControlManager]: safety timer finished");
+    3555           0 :           break;
+    3556             :         }
+    3557             :       }
+    3558             : 
+    3559             :       // we have to also for the oneshot control timer to finish
+    3560           0 :       while (running_async_control_) {
+    3561             : 
+    3562           0 :         ROS_DEBUG("[ControlManager]: waiting for control timer to finish");
+    3563           0 :         ros::Duration wait(0.001);
+    3564           0 :         wait.sleep();
+    3565             : 
+    3566           0 :         if (!running_async_control_) {
+    3567           0 :           ROS_DEBUG("[ControlManager]: control timer finished");
+    3568           0 :           break;
+    3569             :         }
+    3570             :       }
+    3571             : 
+    3572             :       {
+    3573           0 :         std::scoped_lock lock(mutex_controller_list_, mutex_tracker_list_);
+    3574             : 
+    3575           0 :         tracker_list_.at(active_tracker_idx_)->switchOdometrySource(uav_state_odom);
+    3576           0 :         controller_list_.at(active_controller_idx_)->switchOdometrySource(uav_state_odom);
+    3577             :       }
+    3578             :     }
+    3579             :   }
+    3580             : 
+    3581             :   //}
+    3582             : 
+    3583             :   // | ----------- copy the odometry to the uav_state ----------- |
+    3584             : 
+    3585             :   {
+    3586           0 :     std::scoped_lock lock(mutex_uav_state_);
+    3587             : 
+    3588           0 :     previous_uav_state_ = uav_state_;
+    3589             : 
+    3590           0 :     uav_state_ = mrs_msgs::UavState();
+    3591             : 
+    3592           0 :     uav_state_.header           = odom->header;
+    3593           0 :     uav_state_.pose             = odom->pose.pose;
+    3594           0 :     uav_state_.velocity.angular = odom->twist.twist.angular;
+    3595             : 
+    3596             :     // transform the twist into the header's frame
+    3597             :     {
+    3598             :       // the velocity from the odometry
+    3599           0 :       geometry_msgs::Vector3Stamped speed_child_frame;
+    3600           0 :       speed_child_frame.header.frame_id = odom->child_frame_id;
+    3601           0 :       speed_child_frame.header.stamp    = odom->header.stamp;
+    3602           0 :       speed_child_frame.vector.x        = odom->twist.twist.linear.x;
+    3603           0 :       speed_child_frame.vector.y        = odom->twist.twist.linear.y;
+    3604           0 :       speed_child_frame.vector.z        = odom->twist.twist.linear.z;
+    3605             : 
+    3606           0 :       auto res = transformer_->transformSingle(speed_child_frame, odom->header.frame_id);
+    3607             : 
+    3608           0 :       if (res) {
+    3609           0 :         uav_state_.velocity.linear.x = res.value().vector.x;
+    3610           0 :         uav_state_.velocity.linear.y = res.value().vector.y;
+    3611           0 :         uav_state_.velocity.linear.z = res.value().vector.z;
+    3612             :       } else {
+    3613           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not transform the odometry speed from '%s' to '%s'", odom->child_frame_id.c_str(),
+    3614             :                            odom->header.frame_id.c_str());
+    3615           0 :         return;
+    3616             :       }
+    3617             :     }
+    3618             : 
+    3619             :     // calculate the euler angles
+    3620           0 :     std::tie(uav_roll_, uav_pitch_, uav_yaw_) = mrs_lib::AttitudeConverter(odom->pose.pose.orientation);
+    3621             : 
+    3622             :     try {
+    3623           0 :       uav_heading_ = mrs_lib::AttitudeConverter(odom->pose.pose.orientation).getHeading();
+    3624             :     }
+    3625           0 :     catch (...) {
+    3626           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not calculate UAV heading");
+    3627             :     }
+    3628             : 
+    3629           0 :     transformer_->setDefaultFrame(odom->header.frame_id);
+    3630             : 
+    3631           0 :     got_uav_state_ = true;
+    3632             :   }
+    3633             : 
+    3634             :   // run the control loop asynchronously in an OneShotTimer
+    3635             :   // but only if its not already running
+    3636           0 :   if (!running_async_control_) {
+    3637             : 
+    3638           0 :     running_async_control_ = true;
+    3639             : 
+    3640           0 :     async_control_result_ = std::async(std::launch::async, &ControlManager::asyncControl, this);
+    3641             :   }
+    3642             : }
+    3643             : 
+    3644             : //}
+    3645             : 
+    3646             : /* //{ callbackUavState() */
+    3647             : 
+    3648      176221 : void ControlManager::callbackUavState(const mrs_msgs::UavState::ConstPtr msg) {
+    3649             : 
+    3650      176221 :   if (!is_initialized_) {
+    3651       22082 :     return;
+    3652             :   }
+    3653             : 
+    3654      352442 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackUavState");
+    3655      352442 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackUavState", scope_timer_logger_, scope_timer_enabled_);
+    3656             : 
+    3657      176221 :   mrs_msgs::UavStateConstPtr uav_state = msg;
+    3658             : 
+    3659             :   // | ------------------ check for time stamp ------------------ |
+    3660             : 
+    3661             :   {
+    3662      176221 :     std::scoped_lock lock(mutex_uav_state_);
+    3663             : 
+    3664      176221 :     if (uav_state_.header.stamp == uav_state->header.stamp) {
+    3665       22082 :       return;
+    3666             :     }
+    3667             :   }
+    3668             : 
+    3669             :   // | --------------------- check for nans --------------------- |
+    3670             : 
+    3671      154139 :   if (!validateUavState(*uav_state, "ControlManager", "uav_state")) {
+    3672           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: incoming 'uav_state' contains invalid values, throwing it away");
+    3673           0 :     return;
+    3674             :   }
+    3675             : 
+    3676             :   // | -------------------- check for hiccups ------------------- |
+    3677             : 
+    3678             :   /* hickup detection //{ */
+    3679             : 
+    3680      154139 :   double alpha               = 0.99;
+    3681      154139 :   double alpha2              = 0.666;
+    3682      154139 :   double uav_state_count_lim = 1000;
+    3683             : 
+    3684      154139 :   double uav_state_dt = (ros::Time::now() - previous_uav_state_.header.stamp).toSec();
+    3685             : 
+    3686             :   // belive only reasonable numbers
+    3687      154139 :   if (uav_state_dt <= 1.0) {
+    3688             : 
+    3689      153921 :     uav_state_avg_dt_ = alpha * uav_state_avg_dt_ + (1 - alpha) * uav_state_dt;
+    3690             : 
+    3691      153921 :     if (uav_state_count_ < uav_state_count_lim) {
+    3692       83385 :       uav_state_count_++;
+    3693             :     }
+    3694             :   }
+    3695             : 
+    3696      154139 :   if (uav_state_count_ == uav_state_count_lim) {
+    3697             : 
+    3698             :     /* ROS_INFO_STREAM("[ControlManager]: uav_state_dt = " << uav_state_dt); */
+    3699             : 
+    3700       70599 :     if (uav_state_dt < uav_state_avg_dt_ && uav_state_dt > 0.0001) {
+    3701             : 
+    3702       31843 :       uav_state_hiccup_factor_ = alpha2 * uav_state_hiccup_factor_ + (1 - alpha2) * (uav_state_avg_dt_ / uav_state_dt);
+    3703             : 
+    3704       38756 :     } else if (uav_state_avg_dt_ > 0.0001) {
+    3705             : 
+    3706       38756 :       uav_state_hiccup_factor_ = alpha2 * uav_state_hiccup_factor_ + (1 - alpha2) * (uav_state_dt / uav_state_avg_dt_);
+    3707             :     }
+    3708             : 
+    3709       70599 :     if (uav_state_hiccup_factor_ > 3.141592653) {
+    3710             : 
+    3711             :       /* ROS_ERROR_STREAM_THROTTLE(0.1, "[ControlManager]: hiccup factor = " << uav_state_hiccup_factor_); */
+    3712             : 
+    3713           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: ");
+    3714           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // | ------------------------- WARNING ------------------------ |");
+    3715           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |                                                            |");
+    3716           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |            UAV_STATE has a large hiccup factor!            |");
+    3717           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |           hint, hint: you are probably rosbagging          |");
+    3718           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |           lot of data or publishing lot of large           |");
+    3719           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |          messages without mutual nodelet managers.         |");
+    3720           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |                                                            |");
+    3721           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // | ------------------------- WARNING ------------------------ |");
+    3722           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: ");
+    3723             :     }
+    3724             :   }
+    3725             : 
+    3726             :   //}
+    3727             : 
+    3728             :   // | ---------------------- frame switch ---------------------- |
+    3729             : 
+    3730             :   /* frame switch //{ */
+    3731             : 
+    3732             :   // | ----- check for change in odometry frame of reference ---- |
+    3733             : 
+    3734      154139 :   if (got_uav_state_) {
+    3735             : 
+    3736      154030 :     if (uav_state->estimator_iteration != uav_state_.estimator_iteration) {
+    3737             : 
+    3738           5 :       ROS_INFO("[ControlManager]: detecting switch of odometry frame");
+    3739             :       {
+    3740          10 :         std::scoped_lock lock(mutex_uav_state_);
+    3741             : 
+    3742           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,
+    3743             :                  uav_state_.pose.position.z, uav_heading_);
+    3744             :       }
+    3745             : 
+    3746           5 :       odometry_switch_in_progress_ = true;
+    3747             : 
+    3748             :       // we have to stop safety timer, otherwise it will interfere
+    3749           5 :       ROS_DEBUG("[ControlManager]: stopping the safety timer");
+    3750           5 :       timer_safety_.stop();
+    3751           5 :       ROS_DEBUG("[ControlManager]: safety timer stopped");
+    3752             : 
+    3753             :       // wait for the safety timer to stop if its running
+    3754           5 :       while (running_safety_timer_) {
+    3755             : 
+    3756           0 :         ROS_DEBUG("[ControlManager]: waiting for safety timer to finish");
+    3757           0 :         ros::Duration wait(0.001);
+    3758           0 :         wait.sleep();
+    3759             : 
+    3760           0 :         if (!running_safety_timer_) {
+    3761           0 :           ROS_DEBUG("[ControlManager]: safety timer finished");
+    3762           0 :           break;
+    3763             :         }
+    3764             :       }
+    3765             : 
+    3766             :       // we have to also for the oneshot control timer to finish
+    3767           5 :       while (running_async_control_) {
+    3768             : 
+    3769           0 :         ROS_DEBUG("[ControlManager]: waiting for control timer to finish");
+    3770           0 :         ros::Duration wait(0.001);
+    3771           0 :         wait.sleep();
+    3772             : 
+    3773           0 :         if (!running_async_control_) {
+    3774           0 :           ROS_DEBUG("[ControlManager]: control timer finished");
+    3775           0 :           break;
+    3776             :         }
+    3777             :       }
+    3778             : 
+    3779             :       {
+    3780          10 :         std::scoped_lock lock(mutex_controller_list_, mutex_tracker_list_);
+    3781             : 
+    3782           5 :         tracker_list_.at(active_tracker_idx_)->switchOdometrySource(*uav_state);
+    3783           5 :         controller_list_.at(active_controller_idx_)->switchOdometrySource(*uav_state);
+    3784             :       }
+    3785             :     }
+    3786             :   }
+    3787             : 
+    3788             :   //}
+    3789             : 
+    3790             :   // --------------------------------------------------------------
+    3791             :   // |           copy the UavState message for later use          |
+    3792             :   // --------------------------------------------------------------
+    3793             : 
+    3794             :   {
+    3795      154139 :     std::scoped_lock lock(mutex_uav_state_);
+    3796             : 
+    3797      154139 :     previous_uav_state_ = uav_state_;
+    3798             : 
+    3799      154139 :     uav_state_ = *uav_state;
+    3800             : 
+    3801      154139 :     std::tie(uav_roll_, uav_pitch_, uav_yaw_) = mrs_lib::AttitudeConverter(uav_state_.pose.orientation);
+    3802             : 
+    3803             :     try {
+    3804      154139 :       uav_heading_ = mrs_lib::AttitudeConverter(uav_state_.pose.orientation).getHeading();
+    3805             :     }
+    3806           0 :     catch (...) {
+    3807           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not calculate UAV heading, not updating it");
+    3808             :     }
+    3809             : 
+    3810      154139 :     transformer_->setDefaultFrame(uav_state->header.frame_id);
+    3811             : 
+    3812      154139 :     got_uav_state_ = true;
+    3813             :   }
+    3814             : 
+    3815             :   // run the control loop asynchronously in an OneShotTimer
+    3816             :   // but only if its not already running
+    3817      154139 :   if (!running_async_control_) {
+    3818             : 
+    3819      151343 :     running_async_control_ = true;
+    3820             : 
+    3821      151343 :     async_control_result_ = std::async(std::launch::async, &ControlManager::asyncControl, this);
+    3822             :   }
+    3823             : }
+    3824             : 
+    3825             : //}
+    3826             : 
+    3827             : /* //{ callbackGNSS() */
+    3828             : 
+    3829       79573 : void ControlManager::callbackGNSS(const sensor_msgs::NavSatFix::ConstPtr msg) {
+    3830             : 
+    3831       79573 :   if (!is_initialized_) {
+    3832          99 :     return;
+    3833             :   }
+    3834             : 
+    3835      238422 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGNSS");
+    3836      238422 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGNSS", scope_timer_logger_, scope_timer_enabled_);
+    3837             : 
+    3838       79474 :   transformer_->setLatLon(msg->latitude, msg->longitude);
+    3839             : }
+    3840             : 
+    3841             : //}
+    3842             : 
+    3843             : /* callbackJoystick() //{ */
+    3844             : 
+    3845           0 : void ControlManager::callbackJoystick(const sensor_msgs::Joy::ConstPtr msg) {
+    3846             : 
+    3847           0 :   if (!is_initialized_) {
+    3848           0 :     return;
+    3849             :   }
+    3850             : 
+    3851           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackJoystick");
+    3852           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackJoystick", scope_timer_logger_, scope_timer_enabled_);
+    3853             : 
+    3854             :   // copy member variables
+    3855           0 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    3856           0 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    3857             : 
+    3858           0 :   sensor_msgs::JoyConstPtr joystick_data = msg;
+    3859             : 
+    3860             :   // TODO check if the array is smaller than the largest idx
+    3861           0 :   if (joystick_data->buttons.size() == 0 || joystick_data->axes.size() == 0) {
+    3862           0 :     return;
+    3863             :   }
+    3864             : 
+    3865             :   // | ---- switching back to fallback tracker and controller --- |
+    3866             : 
+    3867             :   // if any of the A, B, X, Y buttons are pressed when flying with joystick, switch back to fallback controller and tracker
+    3868           0 :   if ((joystick_data->buttons.at(_channel_A_) == 1 || joystick_data->buttons.at(_channel_B_) == 1 || joystick_data->buttons.at(_channel_X_) == 1 ||
+    3869           0 :        joystick_data->buttons.at(_channel_Y_) == 1) &&
+    3870           0 :       active_tracker_idx == _joystick_tracker_idx_ && active_controller_idx == _joystick_controller_idx_) {
+    3871             : 
+    3872           0 :     ROS_INFO("[ControlManager]: switching from joystick to normal control");
+    3873             : 
+    3874           0 :     switchTracker(_joystick_fallback_tracker_name_);
+    3875           0 :     switchController(_joystick_fallback_controller_name_);
+    3876             : 
+    3877           0 :     joystick_goto_enabled_ = false;
+    3878             :   }
+    3879             : 
+    3880             :   // | ------- joystick control activation ------- |
+    3881             : 
+    3882             :   // if start button was pressed
+    3883           0 :   if (joystick_data->buttons.at(_channel_start_) == 1) {
+    3884             : 
+    3885           0 :     if (!joystick_start_pressed_) {
+    3886             : 
+    3887           0 :       ROS_INFO("[ControlManager]: joystick start button pressed");
+    3888             : 
+    3889           0 :       joystick_start_pressed_    = true;
+    3890           0 :       joystick_start_press_time_ = ros::Time::now();
+    3891             :     }
+    3892             : 
+    3893           0 :   } else if (joystick_start_pressed_) {
+    3894             : 
+    3895           0 :     ROS_INFO("[ControlManager]: joystick start button released");
+    3896             : 
+    3897           0 :     joystick_start_pressed_    = false;
+    3898           0 :     joystick_start_press_time_ = ros::Time(0);
+    3899             :   }
+    3900             : 
+    3901             :   // | ---------------- Joystick goto activation ---------------- |
+    3902             : 
+    3903             :   // if back button was pressed
+    3904           0 :   if (joystick_data->buttons.at(_channel_back_) == 1) {
+    3905             : 
+    3906           0 :     if (!joystick_back_pressed_) {
+    3907             : 
+    3908           0 :       ROS_INFO("[ControlManager]: joystick back button pressed");
+    3909             : 
+    3910           0 :       joystick_back_pressed_    = true;
+    3911           0 :       joystick_back_press_time_ = ros::Time::now();
+    3912             :     }
+    3913             : 
+    3914           0 :   } else if (joystick_back_pressed_) {
+    3915             : 
+    3916           0 :     ROS_INFO("[ControlManager]: joystick back button released");
+    3917             : 
+    3918           0 :     joystick_back_pressed_    = false;
+    3919           0 :     joystick_back_press_time_ = ros::Time(0);
+    3920             :   }
+    3921             : 
+    3922             :   // | ------------------------ Failsafes ----------------------- |
+    3923             : 
+    3924             :   // if LT and RT buttons are both pressed down
+    3925           0 :   if (joystick_data->axes.at(_channel_LT_) < -0.99 && joystick_data->axes.at(_channel_RT_) < -0.99) {
+    3926             : 
+    3927           0 :     if (!joystick_failsafe_pressed_) {
+    3928             : 
+    3929           0 :       ROS_INFO("[ControlManager]: joystick Failsafe pressed");
+    3930             : 
+    3931           0 :       joystick_failsafe_pressed_    = true;
+    3932           0 :       joystick_failsafe_press_time_ = ros::Time::now();
+    3933             :     }
+    3934             : 
+    3935           0 :   } else if (joystick_failsafe_pressed_) {
+    3936             : 
+    3937           0 :     ROS_INFO("[ControlManager]: joystick Failsafe released");
+    3938             : 
+    3939           0 :     joystick_failsafe_pressed_    = false;
+    3940           0 :     joystick_failsafe_press_time_ = ros::Time(0);
+    3941             :   }
+    3942             : 
+    3943             :   // if left and right joypads are both pressed down
+    3944           0 :   if (joystick_data->buttons.at(_channel_L_joy_) == 1 && joystick_data->buttons.at(_channel_R_joy_) == 1) {
+    3945             : 
+    3946           0 :     if (!joystick_eland_pressed_) {
+    3947             : 
+    3948           0 :       ROS_INFO("[ControlManager]: joystick eland pressed");
+    3949             : 
+    3950           0 :       joystick_eland_pressed_    = true;
+    3951           0 :       joystick_eland_press_time_ = ros::Time::now();
+    3952             :     }
+    3953             : 
+    3954           0 :   } else if (joystick_eland_pressed_) {
+    3955             : 
+    3956           0 :     ROS_INFO("[ControlManager]: joystick eland released");
+    3957             : 
+    3958           0 :     joystick_eland_pressed_    = false;
+    3959           0 :     joystick_eland_press_time_ = ros::Time(0);
+    3960             :   }
+    3961             : }
+    3962             : 
+    3963             : //}
+    3964             : 
+    3965             : /* //{ callbackHwApiStatus() */
+    3966             : 
+    3967      452443 : void ControlManager::callbackHwApiStatus(const mrs_msgs::HwApiStatus::ConstPtr msg) {
+    3968             : 
+    3969      452443 :   if (!is_initialized_) {
+    3970         274 :     return;
+    3971             :   }
+    3972             : 
+    3973     1356507 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackHwApiStatus");
+    3974     1356507 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackHwApiStatus", scope_timer_logger_, scope_timer_enabled_);
+    3975             : 
+    3976      904338 :   mrs_msgs::HwApiStatusConstPtr state = msg;
+    3977             : 
+    3978             :   // | ------ detect and print the changes in offboard mode ----- |
+    3979      452169 :   if (state->offboard) {
+    3980             : 
+    3981      313794 :     if (!offboard_mode_) {
+    3982         103 :       offboard_mode_          = true;
+    3983         103 :       offboard_mode_was_true_ = true;
+    3984         103 :       ROS_INFO("[ControlManager]: detected: OFFBOARD mode ON");
+    3985             :     }
+    3986             : 
+    3987             :   } else {
+    3988             : 
+    3989      138375 :     if (offboard_mode_) {
+    3990           0 :       offboard_mode_ = false;
+    3991           0 :       ROS_INFO("[ControlManager]: detected: OFFBOARD mode OFF");
+    3992             :     }
+    3993             :   }
+    3994             : 
+    3995             :   // | --------- detect and print the changes in arming --------- |
+    3996      452169 :   if (state->armed == true) {
+    3997             : 
+    3998      327603 :     if (!armed_) {
+    3999         108 :       armed_ = true;
+    4000         108 :       ROS_INFO("[ControlManager]: detected: vehicle ARMED");
+    4001             :     }
+    4002             : 
+    4003             :   } else {
+    4004             : 
+    4005      124566 :     if (armed_) {
+    4006          20 :       armed_ = false;
+    4007          20 :       ROS_INFO("[ControlManager]: detected: vehicle DISARMED");
+    4008             :     }
+    4009             :   }
+    4010             : }
+    4011             : 
+    4012             : //}
+    4013             : 
+    4014             : /* //{ callbackRC() */
+    4015             : 
+    4016       24855 : void ControlManager::callbackRC(const mrs_msgs::HwApiRcChannels::ConstPtr msg) {
+    4017             : 
+    4018       24855 :   if (!is_initialized_) {
+    4019           0 :     return;
+    4020             :   }
+    4021             : 
+    4022       74565 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackRC");
+    4023       74565 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackRC", scope_timer_logger_, scope_timer_enabled_);
+    4024             : 
+    4025       49710 :   mrs_msgs::HwApiRcChannelsConstPtr rc = msg;
+    4026             : 
+    4027       24855 :   ROS_INFO_ONCE("[ControlManager]: getting RC channels");
+    4028             : 
+    4029             :   // | ------------------- rc joystic control ------------------- |
+    4030             : 
+    4031             :   // when the switch change its position
+    4032       24855 :   if (_rc_goto_enabled_) {
+    4033             : 
+    4034       24855 :     if (_rc_joystick_channel_ >= int(rc->channels.size())) {
+    4035             : 
+    4036           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: RC joystick activation channel number (%d) is out of range [0-%d]", _rc_joystick_channel_,
+    4037             :                          int(rc->channels.size()));
+    4038             : 
+    4039             :     } else {
+    4040             : 
+    4041       24855 :       bool channel_low  = rc->channels.at(_rc_joystick_channel_) < (0.5 - RC_DEADBAND) ? true : false;
+    4042       24855 :       bool channel_high = rc->channels.at(_rc_joystick_channel_) > (0.5 + RC_DEADBAND) ? true : false;
+    4043             : 
+    4044       24855 :       if (channel_low) {
+    4045       22240 :         rc_joystick_channel_was_low_ = true;
+    4046             :       }
+    4047             : 
+    4048             :       // rc control activation
+    4049       24855 :       if (!rc_goto_active_) {
+    4050             : 
+    4051       22240 :         if (rc_joystick_channel_last_value_ < (0.5 - RC_DEADBAND) && channel_high) {
+    4052             : 
+    4053           2 :           if (isFlyingNormally()) {
+    4054             : 
+    4055           2 :             ROS_INFO_THROTTLE(1.0, "[ControlManager]: activating RC joystick");
+    4056             : 
+    4057           2 :             callbacks_enabled_ = false;
+    4058             : 
+    4059           2 :             std_srvs::SetBoolRequest req_goto_out;
+    4060           2 :             req_goto_out.data = false;
+    4061             : 
+    4062           2 :             std_srvs::SetBoolRequest req_enable_callbacks;
+    4063           2 :             req_enable_callbacks.data = callbacks_enabled_;
+    4064             : 
+    4065             :             {
+    4066           4 :               std::scoped_lock lock(mutex_tracker_list_);
+    4067             : 
+    4068             :               // disable callbacks of all trackers
+    4069          14 :               for (int i = 0; i < int(tracker_list_.size()); i++) {
+    4070          12 :                 tracker_list_.at(i)->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4071             :               }
+    4072             :             }
+    4073             : 
+    4074           2 :             rc_goto_active_ = true;
+    4075             : 
+    4076             :           } else {
+    4077             : 
+    4078           0 :             ROS_WARN_THROTTLE(1.0, "[ControlManager]: can not activate RC joystick, not flying normally");
+    4079           2 :           }
+    4080             : 
+    4081       22238 :         } else if (channel_high && !rc_joystick_channel_was_low_) {
+    4082             : 
+    4083           0 :           ROS_WARN_THROTTLE(1.0, "[ControlManager]: can not activate RC joystick, the switch is ON from the beginning");
+    4084             :         }
+    4085             :       }
+    4086             : 
+    4087             :       // rc control deactivation
+    4088       24855 :       if (rc_goto_active_ && channel_low) {
+    4089             : 
+    4090           2 :         ROS_INFO("[ControlManager]: deactivating RC joystick");
+    4091             : 
+    4092           2 :         callbacks_enabled_ = true;
+    4093             : 
+    4094           2 :         std_srvs::SetBoolRequest req_goto_out;
+    4095           2 :         req_goto_out.data = true;
+    4096             : 
+    4097           2 :         std_srvs::SetBoolRequest req_enable_callbacks;
+    4098           2 :         req_enable_callbacks.data = callbacks_enabled_;
+    4099             : 
+    4100             :         {
+    4101           4 :           std::scoped_lock lock(mutex_tracker_list_);
+    4102             : 
+    4103             :           // enable callbacks of all trackers
+    4104          14 :           for (int i = 0; i < int(tracker_list_.size()); i++) {
+    4105          12 :             tracker_list_.at(i)->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4106             :           }
+    4107             :         }
+    4108             : 
+    4109           2 :         rc_goto_active_ = false;
+    4110             :       }
+    4111             : 
+    4112             :       // do not forget to update the last... variable
+    4113             :       // only do that if its out of the deadband
+    4114       24855 :       if (channel_high || channel_low) {
+    4115       24855 :         rc_joystick_channel_last_value_ = rc->channels.at(_rc_joystick_channel_);
+    4116             :       }
+    4117             :     }
+    4118             :   }
+    4119             : 
+    4120             :   // | ----------------- RC escalating failsafe ----------------- |
+    4121             : 
+    4122       24855 :   if (_rc_escalating_failsafe_enabled_) {
+    4123             : 
+    4124       24855 :     if (_rc_escalating_failsafe_channel_ >= int(rc->channels.size())) {
+    4125             : 
+    4126           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: RC eland channel number (%d) is out of range [0-%d]", _rc_escalating_failsafe_channel_,
+    4127             :                          int(rc->channels.size()));
+    4128             : 
+    4129             :     } else {
+    4130             : 
+    4131       24855 :       if (rc->channels.at(_rc_escalating_failsafe_channel_) >= _rc_escalating_failsafe_threshold_) {
+    4132             : 
+    4133         140 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: triggering escalating failsafe by RC");
+    4134             : 
+    4135         280 :         auto [success, message] = escalatingFailsafe();
+    4136             : 
+    4137         140 :         if (success) {
+    4138           3 :           rc_escalating_failsafe_triggered_ = true;
+    4139             :         }
+    4140             :       }
+    4141             :     }
+    4142             :   }
+    4143             : }
+    4144             : 
+    4145             : //}
+    4146             : 
+    4147             : // | --------------------- topic timeouts --------------------- |
+    4148             : 
+    4149             : /* timeoutUavState() //{ */
+    4150             : 
+    4151           0 : void ControlManager::timeoutUavState(const double& missing_for) {
+    4152             : 
+    4153           0 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    4154             : 
+    4155           0 :   if (output_enabled_ && last_control_output.control_output && !failsafe_triggered_) {
+    4156             : 
+    4157             :     // We need to fire up timerFailsafe, which will regularly trigger the controllers
+    4158             :     // in place of the callbackUavState/callbackOdometry().
+    4159             : 
+    4160           0 :     ROS_ERROR_THROTTLE(0.1, "[ControlManager]: not receiving uav_state/odometry for %.3f s, initiating failsafe land", missing_for);
+    4161             : 
+    4162           0 :     failsafe();
+    4163             :   }
+    4164           0 : }
+    4165             : 
+    4166             : //}
+    4167             : 
+    4168             : // | -------------------- service callbacks ------------------- |
+    4169             : 
+    4170             : /* //{ callbackSwitchTracker() */
+    4171             : 
+    4172         214 : bool ControlManager::callbackSwitchTracker(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+    4173             : 
+    4174         214 :   if (!is_initialized_) {
+    4175           0 :     return false;
+    4176             :   }
+    4177             : 
+    4178         214 :   if (failsafe_triggered_ || eland_triggered_) {
+    4179             : 
+    4180           0 :     std::stringstream ss;
+    4181           0 :     ss << "can not switch tracker, eland or failsafe active";
+    4182             : 
+    4183           0 :     res.message = ss.str();
+    4184           0 :     res.success = false;
+    4185             : 
+    4186           0 :     ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+    4187             : 
+    4188           0 :     return true;
+    4189             :   }
+    4190             : 
+    4191         214 :   if (rc_goto_active_) {
+    4192             : 
+    4193           0 :     std::stringstream ss;
+    4194           0 :     ss << "can not switch tracker, RC joystick is active";
+    4195             : 
+    4196           0 :     res.message = ss.str();
+    4197           0 :     res.success = false;
+    4198             : 
+    4199           0 :     ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+    4200             : 
+    4201           0 :     return true;
+    4202             :   }
+    4203             : 
+    4204         214 :   auto [success, response] = switchTracker(req.value);
+    4205             : 
+    4206         214 :   res.success = success;
+    4207         214 :   res.message = response;
+    4208             : 
+    4209         214 :   return true;
+    4210             : }
+    4211             : 
+    4212             : //}
+    4213             : 
+    4214             : /* callbackSwitchController() //{ */
+    4215             : 
+    4216         215 : bool ControlManager::callbackSwitchController(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+    4217             : 
+    4218         215 :   if (!is_initialized_) {
+    4219           0 :     return false;
+    4220             :   }
+    4221             : 
+    4222         215 :   if (failsafe_triggered_ || eland_triggered_) {
+    4223             : 
+    4224           0 :     std::stringstream ss;
+    4225           0 :     ss << "can not switch controller, eland or failsafe active";
+    4226             : 
+    4227           0 :     res.message = ss.str();
+    4228           0 :     res.success = false;
+    4229             : 
+    4230           0 :     ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+    4231             : 
+    4232           0 :     return true;
+    4233             :   }
+    4234             : 
+    4235         215 :   if (rc_goto_active_) {
+    4236             : 
+    4237           2 :     std::stringstream ss;
+    4238           2 :     ss << "can not switch controller, RC joystick is active";
+    4239             : 
+    4240           2 :     res.message = ss.str();
+    4241           2 :     res.success = false;
+    4242             : 
+    4243           2 :     ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+    4244             : 
+    4245           2 :     return true;
+    4246             :   }
+    4247             : 
+    4248         213 :   auto [success, response] = switchController(req.value);
+    4249             : 
+    4250         213 :   res.success = success;
+    4251         213 :   res.message = response;
+    4252             : 
+    4253         213 :   return true;
+    4254             : }
+    4255             : 
+    4256             : //}
+    4257             : 
+    4258             : /* //{ callbackSwitchTracker() */
+    4259             : 
+    4260           0 : bool ControlManager::callbackTrackerResetStatic([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4261             : 
+    4262           0 :   if (!is_initialized_) {
+    4263           0 :     return false;
+    4264             :   }
+    4265             : 
+    4266           0 :   std::stringstream message;
+    4267             : 
+    4268           0 :   if (failsafe_triggered_ || eland_triggered_) {
+    4269             : 
+    4270           0 :     message << "can not reset tracker, eland or failsafe active";
+    4271             : 
+    4272           0 :     res.message = message.str();
+    4273           0 :     res.success = false;
+    4274             : 
+    4275           0 :     ROS_WARN_STREAM("[ControlManager]: " << message.str());
+    4276             : 
+    4277           0 :     return true;
+    4278             :   }
+    4279             : 
+    4280             :   // reactivate the current tracker
+    4281             :   {
+    4282           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    4283             : 
+    4284           0 :     std::string tracker_name = _tracker_names_.at(active_tracker_idx_);
+    4285             : 
+    4286           0 :     bool succ = tracker_list_.at(active_tracker_idx_)->resetStatic();
+    4287             : 
+    4288           0 :     if (succ) {
+    4289           0 :       message << "the tracker '" << tracker_name << "' was reset";
+    4290           0 :       ROS_INFO_STREAM("[ControlManager]: " << message.str());
+    4291             :     } else {
+    4292           0 :       message << "the tracker '" << tracker_name << "' reset failed!";
+    4293           0 :       ROS_ERROR_STREAM("[ControlManager]: " << message.str());
+    4294             :     }
+    4295             :   }
+    4296             : 
+    4297           0 :   res.message = message.str();
+    4298           0 :   res.success = true;
+    4299             : 
+    4300           0 :   return true;
+    4301             : }
+    4302             : 
+    4303             : //}
+    4304             : 
+    4305             : /* //{ callbackEHover() */
+    4306             : 
+    4307           2 : bool ControlManager::callbackEHover([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4308             : 
+    4309           2 :   if (!is_initialized_) {
+    4310           0 :     return false;
+    4311             :   }
+    4312             : 
+    4313           2 :   if (failsafe_triggered_ || eland_triggered_) {
+    4314             : 
+    4315           0 :     std::stringstream ss;
+    4316           0 :     ss << "can not switch controller, eland or failsafe active";
+    4317             : 
+    4318           0 :     res.message = ss.str();
+    4319           0 :     res.success = false;
+    4320             : 
+    4321           0 :     ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+    4322             : 
+    4323           0 :     return true;
+    4324             :   }
+    4325             : 
+    4326           2 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: ehover trigger by callback");
+    4327             : 
+    4328           2 :   auto [success, message] = ehover();
+    4329             : 
+    4330           2 :   res.success = success;
+    4331           2 :   res.message = message;
+    4332             : 
+    4333           2 :   return true;
+    4334             : }
+    4335             : 
+    4336             : //}
+    4337             : 
+    4338             : /* callbackFailsafe() //{ */
+    4339             : 
+    4340           4 : bool ControlManager::callbackFailsafe([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4341             : 
+    4342           4 :   if (!is_initialized_) {
+    4343           0 :     return false;
+    4344             :   }
+    4345             : 
+    4346           4 :   if (failsafe_triggered_) {
+    4347             : 
+    4348           0 :     std::stringstream ss;
+    4349           0 :     ss << "can not activate failsafe, it is already active";
+    4350             : 
+    4351           0 :     res.message = ss.str();
+    4352           0 :     res.success = false;
+    4353             : 
+    4354           0 :     ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4355             : 
+    4356           0 :     return true;
+    4357             :   }
+    4358             : 
+    4359           4 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: failsafe triggered by callback");
+    4360             : 
+    4361           4 :   auto [success, message] = failsafe();
+    4362             : 
+    4363           4 :   res.success = success;
+    4364           4 :   res.message = message;
+    4365             : 
+    4366           4 :   return true;
+    4367             : }
+    4368             : 
+    4369             : //}
+    4370             : 
+    4371             : /* callbackFailsafeEscalating() //{ */
+    4372             : 
+    4373           7 : bool ControlManager::callbackFailsafeEscalating([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4374             : 
+    4375           7 :   if (!is_initialized_) {
+    4376           0 :     return false;
+    4377             :   }
+    4378             : 
+    4379           7 :   if (_service_escalating_failsafe_enabled_) {
+    4380             : 
+    4381           7 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: escalating failsafe triggered by callback");
+    4382             : 
+    4383          14 :     auto [success, message] = escalatingFailsafe();
+    4384             : 
+    4385           7 :     res.success = success;
+    4386           7 :     res.message = message;
+    4387             : 
+    4388             :   } else {
+    4389             : 
+    4390           0 :     std::stringstream ss;
+    4391           0 :     ss << "escalating failsafe is disabled";
+    4392             : 
+    4393           0 :     res.success = false;
+    4394           0 :     res.message = ss.str();
+    4395             : 
+    4396           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: %s", ss.str().c_str());
+    4397             :   }
+    4398             : 
+    4399           7 :   return true;
+    4400             : }
+    4401             : 
+    4402             : //}
+    4403             : 
+    4404             : /* //{ callbackELand() */
+    4405             : 
+    4406           2 : bool ControlManager::callbackEland([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4407             : 
+    4408           2 :   if (!is_initialized_) {
+    4409           0 :     return false;
+    4410             :   }
+    4411             : 
+    4412           2 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: eland triggered by callback");
+    4413             : 
+    4414           2 :   auto [success, message] = eland();
+    4415             : 
+    4416           2 :   res.success = success;
+    4417           2 :   res.message = message;
+    4418             : 
+    4419           2 :   return true;
+    4420             : }
+    4421             : 
+    4422             : //}
+    4423             : 
+    4424             : /* //{ callbackParachute() */
+    4425             : 
+    4426           0 : bool ControlManager::callbackParachute([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4427             : 
+    4428           0 :   if (!is_initialized_) {
+    4429           0 :     return false;
+    4430             :   }
+    4431             : 
+    4432           0 :   if (!_parachute_enabled_) {
+    4433             : 
+    4434           0 :     std::stringstream ss;
+    4435           0 :     ss << "parachute disabled";
+    4436           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4437           0 :     res.message = ss.str();
+    4438           0 :     res.success = false;
+    4439             :   }
+    4440             : 
+    4441           0 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: parachute triggered by callback");
+    4442             : 
+    4443           0 :   auto [success, message] = deployParachute();
+    4444             : 
+    4445           0 :   res.success = success;
+    4446           0 :   res.message = message;
+    4447             : 
+    4448           0 :   return true;
+    4449             : }
+    4450             : 
+    4451             : //}
+    4452             : 
+    4453             : /* //{ callbackSetMinZ() */
+    4454             : 
+    4455           0 : bool ControlManager::callbackSetMinZ([[maybe_unused]] mrs_msgs::Float64StampedSrv::Request& req, mrs_msgs::Float64StampedSrv::Response& res) {
+    4456             : 
+    4457           0 :   if (!is_initialized_) {
+    4458           0 :     return false;
+    4459             :   }
+    4460             : 
+    4461           0 :   if (!use_safety_area_) {
+    4462           0 :     res.success = true;
+    4463           0 :     res.message = "safety area is disabled";
+    4464           0 :     return true;
+    4465             :   }
+    4466             : 
+    4467             :   // | -------- transform min_z to the safety area frame -------- |
+    4468             : 
+    4469           0 :   mrs_msgs::ReferenceStamped point;
+    4470           0 :   point.header               = req.header;
+    4471           0 :   point.reference.position.z = req.value;
+    4472             : 
+    4473           0 :   auto result = transformer_->transformSingle(point, _safety_area_vertical_frame_);
+    4474             : 
+    4475           0 :   if (result) {
+    4476             : 
+    4477           0 :     _safety_area_min_z_ = result.value().reference.position.z;
+    4478             : 
+    4479           0 :     res.success = true;
+    4480           0 :     res.message = "safety area's min z updated";
+    4481             : 
+    4482             :   } else {
+    4483             : 
+    4484           0 :     res.success = false;
+    4485           0 :     res.message = "could not transform the value to safety area's vertical frame";
+    4486             :   }
+    4487             : 
+    4488           0 :   return true;
+    4489             : }
+    4490             : 
+    4491             : //}
+    4492             : 
+    4493             : /* //{ callbackToggleOutput() */
+    4494             : 
+    4495         110 : bool ControlManager::callbackToggleOutput(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4496             : 
+    4497         110 :   if (!is_initialized_) {
+    4498           0 :     return false;
+    4499             :   }
+    4500             : 
+    4501         110 :   ROS_INFO("[ControlManager]: toggling output by service");
+    4502             : 
+    4503             :   // copy member variables
+    4504         220 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4505             : 
+    4506         220 :   std::stringstream ss;
+    4507             : 
+    4508         110 :   bool prereq_check = true;
+    4509             : 
+    4510             :   {
+    4511         220 :     mrs_msgs::ReferenceStamped current_coord;
+    4512         110 :     current_coord.header.frame_id      = uav_state.header.frame_id;
+    4513         110 :     current_coord.reference.position.x = uav_state.pose.position.x;
+    4514         110 :     current_coord.reference.position.y = uav_state.pose.position.y;
+    4515             : 
+    4516         110 :     if (!isPointInSafetyArea2d(current_coord)) {
+    4517           1 :       ss << "cannot toggle output, the UAV is outside of the safety area!";
+    4518           1 :       prereq_check = false;
+    4519             :     }
+    4520             :   }
+    4521             : 
+    4522         110 :   if (req.data && (failsafe_triggered_ || eland_triggered_ || rc_escalating_failsafe_triggered_)) {
+    4523           0 :     ss << "cannot toggle output ON, we landed in emergency";
+    4524           0 :     prereq_check = false;
+    4525             :   }
+    4526             : 
+    4527         110 :   if (!sh_hw_api_status_.hasMsg() || (ros::Time::now() - sh_hw_api_status_.lastMsgTime()).toSec() > 1.0) {
+    4528           0 :     ss << "cannot toggle output ON, missing HW API status!";
+    4529           0 :     prereq_check = false;
+    4530             :   }
+    4531             : 
+    4532         110 :   if (!prereq_check) {
+    4533             : 
+    4534           1 :     res.message = ss.str();
+    4535           1 :     res.success = false;
+    4536             : 
+    4537           1 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4538             : 
+    4539           1 :     return false;
+    4540             : 
+    4541             :   } else {
+    4542             : 
+    4543         109 :     toggleOutput(req.data);
+    4544             : 
+    4545         109 :     ss << "Output: " << (output_enabled_ ? "ON" : "OFF");
+    4546         109 :     res.message = ss.str();
+    4547         109 :     res.success = true;
+    4548             : 
+    4549         109 :     ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4550             : 
+    4551         109 :     publishDiagnostics();
+    4552             : 
+    4553         109 :     return true;
+    4554             :   }
+    4555             : }
+    4556             : 
+    4557             : //}
+    4558             : 
+    4559             : /* callbackArm() //{ */
+    4560             : 
+    4561           7 : bool ControlManager::callbackArm(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4562             : 
+    4563           7 :   if (!is_initialized_) {
+    4564           0 :     return false;
+    4565             :   }
+    4566             : 
+    4567           7 :   ROS_INFO("[ControlManager]: arming by service");
+    4568             : 
+    4569          14 :   std::stringstream ss;
+    4570             : 
+    4571           7 :   if (failsafe_triggered_ || eland_triggered_) {
+    4572             : 
+    4573           0 :     ss << "can not " << (req.data ? "arm" : "disarm") << ", eland or failsafe active";
+    4574             : 
+    4575           0 :     res.message = ss.str();
+    4576           0 :     res.success = false;
+    4577             : 
+    4578           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4579             : 
+    4580           0 :     return true;
+    4581             :   }
+    4582             : 
+    4583           7 :   if (req.data) {
+    4584             : 
+    4585           0 :     ss << "this service is not allowed to arm the UAV";
+    4586           0 :     res.success = false;
+    4587           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4588             : 
+    4589             :   } else {
+    4590             : 
+    4591          14 :     auto [success, message] = arming(false);
+    4592             : 
+    4593           7 :     if (success) {
+    4594             : 
+    4595           7 :       ss << "disarmed";
+    4596           7 :       res.success = true;
+    4597           7 :       ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4598             : 
+    4599             :     } else {
+    4600             : 
+    4601           0 :       ss << "could not disarm: " << message;
+    4602           0 :       res.success = false;
+    4603           0 :       ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4604             :     }
+    4605             :   }
+    4606             : 
+    4607           7 :   res.message = ss.str();
+    4608             : 
+    4609           7 :   return true;
+    4610             : }
+    4611             : 
+    4612             : //}
+    4613             : 
+    4614             : /* //{ callbackEnableCallbacks() */
+    4615             : 
+    4616         102 : bool ControlManager::callbackEnableCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4617             : 
+    4618         102 :   if (!is_initialized_) {
+    4619           0 :     return false;
+    4620             :   }
+    4621             : 
+    4622         102 :   setCallbacks(req.data);
+    4623             : 
+    4624         102 :   std::stringstream ss;
+    4625             : 
+    4626         102 :   ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+    4627             : 
+    4628         102 :   res.message = ss.str();
+    4629         102 :   res.success = true;
+    4630             : 
+    4631         102 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4632             : 
+    4633         102 :   return true;
+    4634             : }
+    4635             : 
+    4636             : //}
+    4637             : 
+    4638             : /* callbackSetConstraints() //{ */
+    4639             : 
+    4640         112 : bool ControlManager::callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrv::Request& req, mrs_msgs::DynamicsConstraintsSrv::Response& res) {
+    4641             : 
+    4642         112 :   if (!is_initialized_) {
+    4643           0 :     res.message = "not initialized";
+    4644           0 :     res.success = false;
+    4645           0 :     return true;
+    4646             :   }
+    4647             : 
+    4648             :   {
+    4649         224 :     std::scoped_lock lock(mutex_constraints_);
+    4650             : 
+    4651         112 :     current_constraints_ = req;
+    4652             : 
+    4653         112 :     auto enforced = enforceControllersConstraints(current_constraints_);
+    4654             : 
+    4655         112 :     if (enforced) {
+    4656           0 :       sanitized_constraints_      = enforced.value();
+    4657           0 :       constraints_being_enforced_ = true;
+    4658             :     } else {
+    4659         112 :       sanitized_constraints_      = req;
+    4660         112 :       constraints_being_enforced_ = false;
+    4661             :     }
+    4662             : 
+    4663         112 :     got_constraints_ = true;
+    4664             : 
+    4665         112 :     setConstraintsToControllers(current_constraints_);
+    4666         112 :     setConstraintsToTrackers(sanitized_constraints_);
+    4667             :   }
+    4668             : 
+    4669         112 :   res.message = "setting constraints";
+    4670         112 :   res.success = true;
+    4671             : 
+    4672         112 :   return true;
+    4673             : }
+    4674             : 
+    4675             : //}
+    4676             : 
+    4677             : /* //{ callbackEmergencyReference() */
+    4678             : 
+    4679         101 : bool ControlManager::callbackEmergencyReference(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res) {
+    4680             : 
+    4681         101 :   if (!is_initialized_) {
+    4682           0 :     return false;
+    4683             :   }
+    4684             : 
+    4685         202 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4686             : 
+    4687         101 :   callbacks_enabled_ = false;
+    4688             : 
+    4689         101 :   mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;
+    4690             : 
+    4691         202 :   std::stringstream ss;
+    4692             : 
+    4693             :   // transform the reference to the current frame
+    4694         202 :   mrs_msgs::ReferenceStamped original_reference;
+    4695         101 :   original_reference.header    = req.header;
+    4696         101 :   original_reference.reference = req.reference;
+    4697             : 
+    4698         202 :   auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    4699             : 
+    4700         101 :   if (!ret) {
+    4701             : 
+    4702           0 :     ss << "the emergency reference could not be transformed";
+    4703             : 
+    4704           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4705           0 :     res.message = ss.str();
+    4706           0 :     res.success = false;
+    4707           0 :     return true;
+    4708             :   }
+    4709             : 
+    4710         101 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    4711             : 
+    4712         101 :   std_srvs::SetBoolRequest req_enable_callbacks;
+    4713             : 
+    4714         101 :   mrs_msgs::ReferenceSrvRequest req_goto_out;
+    4715         101 :   req_goto_out.reference = transformed_reference.reference;
+    4716             : 
+    4717             :   {
+    4718         202 :     std::scoped_lock lock(mutex_tracker_list_);
+    4719             : 
+    4720             :     // disable callbacks of all trackers
+    4721         101 :     req_enable_callbacks.data = false;
+    4722         707 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    4723         606 :       tracker_list_.at(i)->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4724             :     }
+    4725             : 
+    4726             :     // enable the callbacks for the active tracker
+    4727         101 :     req_enable_callbacks.data = true;
+    4728         101 :     tracker_list_.at(active_tracker_idx_)
+    4729         101 :         ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4730             : 
+    4731             :     // call the setReference()
+    4732         101 :     tracker_response = tracker_list_.at(active_tracker_idx_)
+    4733         101 :                            ->setReference(mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(req_goto_out)));
+    4734             : 
+    4735             :     // disable the callbacks back again
+    4736         101 :     req_enable_callbacks.data = false;
+    4737         101 :     tracker_list_.at(active_tracker_idx_)
+    4738         101 :         ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4739             : 
+    4740         101 :     if (tracker_response != mrs_msgs::ReferenceSrvResponse::Ptr()) {
+    4741         101 :       res.message = tracker_response->message;
+    4742         101 :       res.success = tracker_response->success;
+    4743             :     } else {
+    4744           0 :       ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'setReference()' function!";
+    4745           0 :       res.message = ss.str();
+    4746           0 :       res.success = false;
+    4747             :     }
+    4748             :   }
+    4749             : 
+    4750         101 :   return true;
+    4751             : }
+    4752             : 
+    4753             : //}
+    4754             : 
+    4755             : /* callbackPirouette() //{ */
+    4756             : 
+    4757           0 : bool ControlManager::callbackPirouette([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4758             : 
+    4759           0 :   if (!is_initialized_) {
+    4760           0 :     return false;
+    4761             :   }
+    4762             : 
+    4763             :   // copy member variables
+    4764           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4765             : 
+    4766             :   double uav_heading;
+    4767             :   try {
+    4768           0 :     uav_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    4769             :   }
+    4770           0 :   catch (...) {
+    4771           0 :     std::stringstream ss;
+    4772           0 :     ss << "could not calculate the UAV heading to initialize the pirouette";
+    4773             : 
+    4774           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4775             : 
+    4776           0 :     res.message = ss.str();
+    4777           0 :     res.success = false;
+    4778             : 
+    4779           0 :     return false;
+    4780             :   }
+    4781             : 
+    4782           0 :   if (_pirouette_enabled_) {
+    4783           0 :     res.success = false;
+    4784           0 :     res.message = "already active";
+    4785           0 :     return true;
+    4786             :   }
+    4787             : 
+    4788           0 :   if (failsafe_triggered_ || eland_triggered_ || rc_escalating_failsafe_triggered_) {
+    4789             : 
+    4790           0 :     std::stringstream ss;
+    4791           0 :     ss << "can not activate the pirouette, eland or failsafe active";
+    4792             : 
+    4793           0 :     res.message = ss.str();
+    4794           0 :     res.success = false;
+    4795             : 
+    4796           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4797             : 
+    4798           0 :     return true;
+    4799             :   }
+    4800             : 
+    4801           0 :   _pirouette_enabled_ = true;
+    4802             : 
+    4803           0 :   setCallbacks(false);
+    4804             : 
+    4805           0 :   pirouette_initial_heading_ = uav_heading;
+    4806           0 :   pirouette_iterator_        = 0;
+    4807           0 :   timer_pirouette_.start();
+    4808             : 
+    4809           0 :   res.success = true;
+    4810           0 :   res.message = "activated";
+    4811             : 
+    4812           0 :   return true;
+    4813             : }
+    4814             : 
+    4815             : //}
+    4816             : 
+    4817             : /* callbackUseJoystick() //{ */
+    4818             : 
+    4819           0 : bool ControlManager::callbackUseJoystick([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4820             : 
+    4821           0 :   if (!is_initialized_) {
+    4822           0 :     return false;
+    4823             :   }
+    4824             : 
+    4825           0 :   std::stringstream ss;
+    4826             : 
+    4827             :   {
+    4828           0 :     auto [success, response] = switchTracker(_joystick_tracker_name_);
+    4829             : 
+    4830           0 :     if (!success) {
+    4831             : 
+    4832           0 :       ss << "switching to '" << _joystick_tracker_name_ << "' was unsuccessfull: '" << response << "'";
+    4833           0 :       ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4834             : 
+    4835           0 :       res.success = false;
+    4836           0 :       res.message = ss.str();
+    4837             : 
+    4838           0 :       return true;
+    4839             :     }
+    4840             :   }
+    4841             : 
+    4842           0 :   auto [success, response] = switchController(_joystick_controller_name_);
+    4843             : 
+    4844           0 :   if (!success) {
+    4845             : 
+    4846           0 :     ss << "switching to '" << _joystick_controller_name_ << "' was unsuccessfull: '" << response << "'";
+    4847           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4848             : 
+    4849           0 :     res.success = false;
+    4850           0 :     res.message = ss.str();
+    4851             : 
+    4852             :     // switch back to hover tracker
+    4853           0 :     switchTracker(_ehover_tracker_name_);
+    4854             : 
+    4855             :     // switch back to safety controller
+    4856           0 :     switchController(_eland_controller_name_);
+    4857             : 
+    4858           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4859             : 
+    4860           0 :     return true;
+    4861             :   }
+    4862             : 
+    4863           0 :   ss << "switched to joystick control";
+    4864             : 
+    4865           0 :   res.success = true;
+    4866           0 :   res.message = ss.str();
+    4867             : 
+    4868           0 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4869             : 
+    4870           0 :   return true;
+    4871             : }
+    4872             : 
+    4873             : //}
+    4874             : 
+    4875             : /* //{ callbackHover() */
+    4876             : 
+    4877           1 : bool ControlManager::callbackHover([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4878             : 
+    4879           1 :   if (!is_initialized_) {
+    4880           0 :     return false;
+    4881             :   }
+    4882             : 
+    4883           1 :   auto [success, message] = hover();
+    4884             : 
+    4885           1 :   res.success = success;
+    4886           1 :   res.message = message;
+    4887             : 
+    4888           1 :   return true;
+    4889             : }
+    4890             : 
+    4891             : //}
+    4892             : 
+    4893             : /* //{ callbackStartTrajectoryTracking() */
+    4894             : 
+    4895           2 : bool ControlManager::callbackStartTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4896             : 
+    4897           2 :   if (!is_initialized_) {
+    4898           0 :     return false;
+    4899             :   }
+    4900             : 
+    4901           2 :   auto [success, message] = startTrajectoryTracking();
+    4902             : 
+    4903           2 :   res.success = success;
+    4904           2 :   res.message = message;
+    4905             : 
+    4906           2 :   return true;
+    4907             : }
+    4908             : 
+    4909             : //}
+    4910             : 
+    4911             : /* //{ callbackStopTrajectoryTracking() */
+    4912             : 
+    4913           1 : bool ControlManager::callbackStopTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4914             : 
+    4915           1 :   if (!is_initialized_) {
+    4916           0 :     return false;
+    4917             :   }
+    4918             : 
+    4919           1 :   auto [success, message] = stopTrajectoryTracking();
+    4920             : 
+    4921           1 :   res.success = success;
+    4922           1 :   res.message = message;
+    4923             : 
+    4924           1 :   return true;
+    4925             : }
+    4926             : 
+    4927             : //}
+    4928             : 
+    4929             : /* //{ callbackResumeTrajectoryTracking() */
+    4930             : 
+    4931           1 : bool ControlManager::callbackResumeTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4932             : 
+    4933           1 :   if (!is_initialized_) {
+    4934           0 :     return false;
+    4935             :   }
+    4936             : 
+    4937           1 :   auto [success, message] = resumeTrajectoryTracking();
+    4938             : 
+    4939           1 :   res.success = success;
+    4940           1 :   res.message = message;
+    4941             : 
+    4942           1 :   return true;
+    4943             : }
+    4944             : 
+    4945             : //}
+    4946             : 
+    4947             : /* //{ callbackGotoTrajectoryStart() */
+    4948             : 
+    4949           2 : bool ControlManager::callbackGotoTrajectoryStart([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4950             : 
+    4951           2 :   if (!is_initialized_) {
+    4952           0 :     return false;
+    4953             :   }
+    4954             : 
+    4955           2 :   auto [success, message] = gotoTrajectoryStart();
+    4956             : 
+    4957           2 :   res.success = success;
+    4958           2 :   res.message = message;
+    4959             : 
+    4960           2 :   return true;
+    4961             : }
+    4962             : 
+    4963             : //}
+    4964             : 
+    4965             : /* //{ callbackTransformReference() */
+    4966             : 
+    4967           1 : bool ControlManager::callbackTransformReference(mrs_msgs::TransformReferenceSrv::Request& req, mrs_msgs::TransformReferenceSrv::Response& res) {
+    4968             : 
+    4969           1 :   if (!is_initialized_) {
+    4970           0 :     return false;
+    4971             :   }
+    4972             : 
+    4973             :   // transform the reference to the current frame
+    4974           2 :   mrs_msgs::ReferenceStamped transformed_reference = req.reference;
+    4975             : 
+    4976           2 :   if (auto ret = transformer_->transformSingle(transformed_reference, req.frame_id)) {
+    4977             : 
+    4978           1 :     res.reference = ret.value();
+    4979           1 :     res.message   = "transformation successful";
+    4980           1 :     res.success   = true;
+    4981           1 :     return true;
+    4982             : 
+    4983             :   } else {
+    4984             : 
+    4985           0 :     res.message = "the reference could not be transformed";
+    4986           0 :     res.success = false;
+    4987           0 :     return true;
+    4988             :   }
+    4989             : 
+    4990             :   return true;
+    4991             : }
+    4992             : 
+    4993             : //}
+    4994             : 
+    4995             : /* //{ callbackTransformReferenceArray() */
+    4996             : 
+    4997           0 : bool ControlManager::callbackTransformReferenceArray(mrs_msgs::TransformReferenceArraySrv::Request& req, mrs_msgs::TransformReferenceArraySrv::Response& res) {
+    4998             : 
+    4999           0 :   if (!is_initialized_) {
+    5000           0 :     return false;
+    5001             :   }
+    5002             : 
+    5003             :   // transform the reference array to the current frame
+    5004           0 :   const auto tf_opt = transformer_->getTransform(req.array.header.frame_id, req.to_frame_id, req.array.header.stamp);
+    5005           0 :   if (!tf_opt.has_value()) {
+    5006           0 :     res.message = "The reference array could not be transformed";
+    5007           0 :     res.success = false;
+    5008           0 :     return true;
+    5009             :   }
+    5010           0 :   const auto tf = tf_opt.value();
+    5011             : 
+    5012           0 :   res.array.header.stamp = req.array.header.stamp;
+    5013           0 :   res.array.header.frame_id = req.to_frame_id;
+    5014           0 :   res.array.array.reserve(req.array.array.size());
+    5015             : 
+    5016           0 :   for (const auto& ref : req.array.array) {
+    5017             : 
+    5018           0 :     mrs_msgs::ReferenceStamped ref_stamped; 
+    5019           0 :     ref_stamped.header = req.array.header;
+    5020           0 :     ref_stamped.reference = ref;
+    5021             : 
+    5022           0 :     if (auto ret = transformer_->transform(ref_stamped, tf)) {
+    5023             : 
+    5024           0 :       res.array.array.push_back(ret.value().reference);
+    5025             : 
+    5026             :     } else {
+    5027             : 
+    5028           0 :       res.message = "The reference array could not be transformed";
+    5029           0 :       res.success = false;
+    5030           0 :       return true;
+    5031             :     }
+    5032             : 
+    5033             :   }
+    5034             : 
+    5035           0 :   res.message   = "transformation successful";
+    5036           0 :   res.success   = true;
+    5037           0 :   return true;
+    5038             : }
+    5039             : 
+    5040             : //}
+    5041             : 
+    5042             : /* //{ callbackTransformPose() */
+    5043             : 
+    5044           1 : bool ControlManager::callbackTransformPose(mrs_msgs::TransformPoseSrv::Request& req, mrs_msgs::TransformPoseSrv::Response& res) {
+    5045             : 
+    5046           1 :   if (!is_initialized_) {
+    5047           0 :     return false;
+    5048             :   }
+    5049             : 
+    5050             :   // transform the reference to the current frame
+    5051           2 :   geometry_msgs::PoseStamped transformed_pose = req.pose;
+    5052             : 
+    5053           2 :   if (auto ret = transformer_->transformSingle(transformed_pose, req.frame_id)) {
+    5054             : 
+    5055           1 :     res.pose    = ret.value();
+    5056           1 :     res.message = "transformation successful";
+    5057           1 :     res.success = true;
+    5058           1 :     return true;
+    5059             : 
+    5060             :   } else {
+    5061             : 
+    5062           0 :     res.message = "the pose could not be transformed";
+    5063           0 :     res.success = false;
+    5064           0 :     return true;
+    5065             :   }
+    5066             : 
+    5067             :   return true;
+    5068             : }
+    5069             : 
+    5070             : //}
+    5071             : 
+    5072             : /* //{ callbackTransformVector3() */
+    5073             : 
+    5074           1 : bool ControlManager::callbackTransformVector3(mrs_msgs::TransformVector3Srv::Request& req, mrs_msgs::TransformVector3Srv::Response& res) {
+    5075             : 
+    5076           1 :   if (!is_initialized_) {
+    5077           0 :     return false;
+    5078             :   }
+    5079             : 
+    5080             :   // transform the reference to the current frame
+    5081           2 :   geometry_msgs::Vector3Stamped transformed_vector3 = req.vector;
+    5082             : 
+    5083           2 :   if (auto ret = transformer_->transformSingle(transformed_vector3, req.frame_id)) {
+    5084             : 
+    5085           1 :     res.vector  = ret.value();
+    5086           1 :     res.message = "transformation successful";
+    5087           1 :     res.success = true;
+    5088           1 :     return true;
+    5089             : 
+    5090             :   } else {
+    5091             : 
+    5092           0 :     res.message = "the twist could not be transformed";
+    5093           0 :     res.success = false;
+    5094           0 :     return true;
+    5095             :   }
+    5096             : 
+    5097             :   return true;
+    5098             : }
+    5099             : 
+    5100             : //}
+    5101             : 
+    5102             : /* //{ callbackEnableBumper() */
+    5103             : 
+    5104           0 : bool ControlManager::callbackEnableBumper(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    5105             : 
+    5106           0 :   if (!is_initialized_) {
+    5107           0 :     return false;
+    5108             :   }
+    5109             : 
+    5110           0 :   bumper_enabled_ = req.data;
+    5111             : 
+    5112           0 :   std::stringstream ss;
+    5113             : 
+    5114           0 :   ss << "bumper " << (bumper_enabled_ ? "enalbed" : "disabled");
+    5115             : 
+    5116           0 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    5117             : 
+    5118           0 :   res.success = true;
+    5119           0 :   res.message = ss.str();
+    5120             : 
+    5121           0 :   return true;
+    5122             : }
+    5123             : 
+    5124             : //}
+    5125             : 
+    5126             : /* //{ callbackUseSafetyArea() */
+    5127             : 
+    5128           0 : bool ControlManager::callbackUseSafetyArea(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    5129             : 
+    5130           0 :   if (!is_initialized_) {
+    5131           0 :     return false;
+    5132             :   }
+    5133             : 
+    5134           0 :   use_safety_area_ = req.data;
+    5135             : 
+    5136           0 :   std::stringstream ss;
+    5137             : 
+    5138           0 :   ss << "safety area " << (use_safety_area_ ? "enabled" : "disabled");
+    5139             : 
+    5140           0 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    5141             : 
+    5142           0 :   res.success = true;
+    5143           0 :   res.message = ss.str();
+    5144             : 
+    5145           0 :   return true;
+    5146             : }
+    5147             : 
+    5148             : //}
+    5149             : 
+    5150             : /* //{ callbackGetMinZ() */
+    5151             : 
+    5152           0 : bool ControlManager::callbackGetMinZ([[maybe_unused]] mrs_msgs::GetFloat64::Request& req, mrs_msgs::GetFloat64::Response& res) {
+    5153             : 
+    5154           0 :   if (!is_initialized_) {
+    5155           0 :     return false;
+    5156             :   }
+    5157             : 
+    5158           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5159             : 
+    5160           0 :   res.success = true;
+    5161           0 :   res.value   = getMinZ(uav_state.header.frame_id);
+    5162             : 
+    5163           0 :   return true;
+    5164             : }
+    5165             : 
+    5166             : //}
+    5167             : 
+    5168             : /* //{ callbackValidateReference() */
+    5169             : 
+    5170           4 : bool ControlManager::callbackValidateReference(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res) {
+    5171             : 
+    5172           4 :   if (!is_initialized_) {
+    5173           0 :     res.message = "not initialized";
+    5174           0 :     res.success = false;
+    5175           0 :     return true;
+    5176             :   }
+    5177             : 
+    5178           4 :   if (!validateReference(req.reference.reference, "ControlManager", "reference_for_validation")) {
+    5179           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: NaN detected in variable 'req.reference'!!!");
+    5180           0 :     res.message = "NaNs/infs in input!";
+    5181           0 :     res.success = false;
+    5182           0 :     return true;
+    5183             :   }
+    5184             : 
+    5185             :   // copy member variables
+    5186           8 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5187           8 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5188             : 
+    5189             :   // transform the reference to the current frame
+    5190           8 :   mrs_msgs::ReferenceStamped original_reference;
+    5191           4 :   original_reference.header    = req.reference.header;
+    5192           4 :   original_reference.reference = req.reference.reference;
+    5193             : 
+    5194           8 :   auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    5195             : 
+    5196           4 :   if (!ret) {
+    5197             : 
+    5198           1 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: the reference could not be transformed");
+    5199           1 :     res.message = "the reference could not be transformed";
+    5200           1 :     res.success = false;
+    5201           1 :     return true;
+    5202             :   }
+    5203             : 
+    5204           6 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5205             : 
+    5206           3 :   if (!isPointInSafetyArea3d(transformed_reference)) {
+    5207           2 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the point is outside of the safety area!");
+    5208           2 :     res.message = "the point is outside of the safety area";
+    5209           2 :     res.success = false;
+    5210           2 :     return true;
+    5211             :   }
+    5212             : 
+    5213           1 :   if (last_tracker_cmd) {
+    5214             : 
+    5215           1 :     mrs_msgs::ReferenceStamped from_point;
+    5216           1 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5217           1 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5218           1 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5219           1 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5220             : 
+    5221           1 :     if (!isPathToPointInSafetyArea3d(from_point, transformed_reference)) {
+    5222           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the path is going outside the safety area!");
+    5223           0 :       res.message = "the path is going outside the safety area";
+    5224           0 :       res.success = false;
+    5225           0 :       return true;
+    5226             :     }
+    5227             :   }
+    5228             : 
+    5229           1 :   res.message = "the reference is ok";
+    5230           1 :   res.success = true;
+    5231           1 :   return true;
+    5232             : }
+    5233             : 
+    5234             : //}
+    5235             : 
+    5236             : /* //{ callbackValidateReference2d() */
+    5237             : 
+    5238       10071 : bool ControlManager::callbackValidateReference2d(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res) {
+    5239             : 
+    5240       10071 :   if (!is_initialized_) {
+    5241           0 :     res.message = "not initialized";
+    5242           0 :     res.success = false;
+    5243           0 :     return true;
+    5244             :   }
+    5245             : 
+    5246       10071 :   if (!validateReference(req.reference.reference, "ControlManager", "reference_for_validation")) {
+    5247           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: NaN detected in variable 'req.reference'!!!");
+    5248           0 :     res.message = "NaNs/infs in input!";
+    5249           0 :     res.success = false;
+    5250           0 :     return true;
+    5251             :   }
+    5252             : 
+    5253             :   // copy member variables
+    5254       20142 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5255       20142 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5256             : 
+    5257             :   // transform the reference to the current frame
+    5258       20142 :   mrs_msgs::ReferenceStamped original_reference;
+    5259       10071 :   original_reference.header    = req.reference.header;
+    5260       10071 :   original_reference.reference = req.reference.reference;
+    5261             : 
+    5262       20142 :   auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    5263             : 
+    5264       10071 :   if (!ret) {
+    5265             : 
+    5266           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: the reference could not be transformed");
+    5267           0 :     res.message = "the reference could not be transformed";
+    5268           0 :     res.success = false;
+    5269           0 :     return true;
+    5270             :   }
+    5271             : 
+    5272       20142 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5273             : 
+    5274       10071 :   if (!isPointInSafetyArea2d(transformed_reference)) {
+    5275          75 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the point is outside of the safety area!");
+    5276          75 :     res.message = "the point is outside of the safety area";
+    5277          75 :     res.success = false;
+    5278          75 :     return true;
+    5279             :   }
+    5280             : 
+    5281        9996 :   if (last_tracker_cmd) {
+    5282             : 
+    5283           0 :     mrs_msgs::ReferenceStamped from_point;
+    5284           0 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5285           0 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5286           0 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5287           0 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5288             : 
+    5289           0 :     if (!isPathToPointInSafetyArea2d(from_point, transformed_reference)) {
+    5290           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the path is going outside the safety area!");
+    5291           0 :       res.message = "the path is going outside the safety area";
+    5292           0 :       res.success = false;
+    5293           0 :       return true;
+    5294             :     }
+    5295             :   }
+    5296             : 
+    5297        9996 :   res.message = "the reference is ok";
+    5298        9996 :   res.success = true;
+    5299        9996 :   return true;
+    5300             : }
+    5301             : 
+    5302             : //}
+    5303             : 
+    5304             : /* //{ callbackValidateReferenceArray() */
+    5305             : 
+    5306           2 : bool ControlManager::callbackValidateReferenceArray(mrs_msgs::ValidateReferenceArray::Request& req, mrs_msgs::ValidateReferenceArray::Response& res) {
+    5307             : 
+    5308           2 :   if (!is_initialized_) {
+    5309           0 :     res.message = "not initialized";
+    5310           0 :     return false;
+    5311             :   }
+    5312             : 
+    5313             :   // copy member variables
+    5314           4 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5315           4 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5316             : 
+    5317             :   // get the transformer
+    5318           4 :   auto ret = transformer_->getTransform(uav_state.header.frame_id, req.array.header.frame_id, req.array.header.stamp);
+    5319             : 
+    5320           2 :   if (!ret) {
+    5321             : 
+    5322           1 :     ROS_DEBUG("[ControlManager]: could not find transform for the reference");
+    5323           1 :     res.message = "could not find transform";
+    5324           1 :     return false;
+    5325             :   }
+    5326             : 
+    5327           1 :   geometry_msgs::TransformStamped tf = ret.value();
+    5328             : 
+    5329           5 :   for (int i = 0; i < int(req.array.array.size()); i++) {
+    5330             : 
+    5331           4 :     res.success.push_back(true);
+    5332             : 
+    5333           8 :     mrs_msgs::ReferenceStamped original_reference;
+    5334           4 :     original_reference.header    = req.array.header;
+    5335           4 :     original_reference.reference = req.array.array.at(i);
+    5336             : 
+    5337           4 :     res.success.at(i) = validateReference(original_reference.reference, "ControlManager", "reference_array");
+    5338             : 
+    5339           8 :     auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    5340             : 
+    5341           4 :     if (!ret) {
+    5342             : 
+    5343           0 :       ROS_DEBUG("[ControlManager]: the reference could not be transformed");
+    5344           0 :       res.success.at(i) = false;
+    5345             :     }
+    5346             : 
+    5347           8 :     mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5348             : 
+    5349           4 :     if (!isPointInSafetyArea3d(transformed_reference)) {
+    5350           2 :       res.success.at(i) = false;
+    5351             :     }
+    5352             : 
+    5353           4 :     if (last_tracker_cmd) {
+    5354             : 
+    5355           8 :       mrs_msgs::ReferenceStamped from_point;
+    5356           4 :       from_point.header.frame_id      = uav_state.header.frame_id;
+    5357           4 :       from_point.reference.position.x = last_tracker_cmd->position.x;
+    5358           4 :       from_point.reference.position.y = last_tracker_cmd->position.y;
+    5359           4 :       from_point.reference.position.z = last_tracker_cmd->position.z;
+    5360             : 
+    5361           4 :       if (!isPathToPointInSafetyArea3d(from_point, transformed_reference)) {
+    5362           2 :         res.success.at(i) = false;
+    5363             :       }
+    5364             :     }
+    5365             :   }
+    5366             : 
+    5367           1 :   res.message = "references were checked";
+    5368           1 :   return true;
+    5369             : }
+    5370             : 
+    5371             : //}
+    5372             : 
+    5373             : // | -------------- setpoint topics and services -------------- |
+    5374             : 
+    5375             : /* //{ callbackReferenceService() */
+    5376             : 
+    5377           2 : bool ControlManager::callbackReferenceService(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res) {
+    5378             : 
+    5379           2 :   if (!is_initialized_) {
+    5380           0 :     res.message = "not initialized";
+    5381           0 :     res.success = false;
+    5382           0 :     return true;
+    5383             :   }
+    5384             : 
+    5385           6 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackReferenceService");
+    5386           6 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackReferenceService", scope_timer_logger_, scope_timer_enabled_);
+    5387             : 
+    5388           4 :   mrs_msgs::ReferenceStamped des_reference;
+    5389           2 :   des_reference.header    = req.header;
+    5390           2 :   des_reference.reference = req.reference;
+    5391             : 
+    5392           4 :   auto [success, message] = setReference(des_reference);
+    5393             : 
+    5394           2 :   res.success = success;
+    5395           2 :   res.message = message;
+    5396             : 
+    5397           2 :   return true;
+    5398             : }
+    5399             : 
+    5400             : //}
+    5401             : 
+    5402             : /* //{ callbackReferenceTopic() */
+    5403             : 
+    5404           1 : void ControlManager::callbackReferenceTopic(const mrs_msgs::ReferenceStamped::ConstPtr msg) {
+    5405             : 
+    5406           1 :   if (!is_initialized_) {
+    5407           0 :     return;
+    5408             :   }
+    5409             : 
+    5410           3 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackReferenceTopic");
+    5411           2 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackReferenceTopic", scope_timer_logger_, scope_timer_enabled_);
+    5412             : 
+    5413           1 :   setReference(*msg);
+    5414             : }
+    5415             : 
+    5416             : //}
+    5417             : 
+    5418             : /* //{ callbackVelocityReferenceService() */
+    5419             : 
+    5420         717 : bool ControlManager::callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrv::Request&  req,
+    5421             :                                                       mrs_msgs::VelocityReferenceStampedSrv::Response& res) {
+    5422             : 
+    5423         717 :   if (!is_initialized_) {
+    5424           0 :     res.message = "not initialized";
+    5425           0 :     res.success = false;
+    5426           0 :     return true;
+    5427             :   }
+    5428             : 
+    5429        2151 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackVelocityReferenceService");
+    5430        2151 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackVelocityReferenceService", scope_timer_logger_, scope_timer_enabled_);
+    5431             : 
+    5432        1434 :   mrs_msgs::VelocityReferenceStamped des_reference;
+    5433         717 :   des_reference = req.reference;
+    5434             : 
+    5435         717 :   auto [success, message] = setVelocityReference(des_reference);
+    5436             : 
+    5437         717 :   res.success = success;
+    5438         717 :   res.message = message;
+    5439             : 
+    5440         717 :   return true;
+    5441             : }
+    5442             : 
+    5443             : //}
+    5444             : 
+    5445             : /* //{ callbackVelocityReferenceTopic() */
+    5446             : 
+    5447          92 : void ControlManager::callbackVelocityReferenceTopic(const mrs_msgs::VelocityReferenceStamped::ConstPtr msg) {
+    5448             : 
+    5449          92 :   if (!is_initialized_) {
+    5450           0 :     return;
+    5451             :   }
+    5452             : 
+    5453         276 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackVelocityReferenceTopic");
+    5454         184 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackVelocityReferenceTopic", scope_timer_logger_, scope_timer_enabled_);
+    5455             : 
+    5456          92 :   setVelocityReference(*msg);
+    5457             : }
+    5458             : 
+    5459             : //}
+    5460             : 
+    5461             : /* //{ callbackTrajectoryReferenceService() */
+    5462             : 
+    5463           4 : bool ControlManager::callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrv::Request& req, mrs_msgs::TrajectoryReferenceSrv::Response& res) {
+    5464             : 
+    5465           4 :   if (!is_initialized_) {
+    5466           0 :     res.message = "not initialized";
+    5467           0 :     res.success = false;
+    5468           0 :     return true;
+    5469             :   }
+    5470             : 
+    5471          12 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackTrajectoryReferenceService");
+    5472          12 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackTrajectoryReferenceService", scope_timer_logger_, scope_timer_enabled_);
+    5473             : 
+    5474           8 :   auto [success, message, modified, tracker_names, tracker_successes, tracker_messages] = setTrajectoryReference(req.trajectory);
+    5475             : 
+    5476           4 :   res.success          = success;
+    5477           4 :   res.message          = message;
+    5478           4 :   res.modified         = modified;
+    5479           4 :   res.tracker_names    = tracker_names;
+    5480           4 :   res.tracker_messages = tracker_messages;
+    5481             : 
+    5482          28 :   for (size_t i = 0; i < tracker_successes.size(); i++) {
+    5483          24 :     res.tracker_successes.push_back(tracker_successes.at(i));
+    5484             :   }
+    5485             : 
+    5486           4 :   return true;
+    5487             : }
+    5488             : 
+    5489             : //}
+    5490             : 
+    5491             : /* //{ callbackTrajectoryReferenceTopic() */
+    5492             : 
+    5493           2 : void ControlManager::callbackTrajectoryReferenceTopic(const mrs_msgs::TrajectoryReference::ConstPtr msg) {
+    5494             : 
+    5495           2 :   if (!is_initialized_) {
+    5496           0 :     return;
+    5497             :   }
+    5498             : 
+    5499           6 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackTrajectoryReferenceTopic");
+    5500           4 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackTrajectoryReferenceTopic", scope_timer_logger_, scope_timer_enabled_);
+    5501             : 
+    5502           2 :   setTrajectoryReference(*msg);
+    5503             : }
+    5504             : 
+    5505             : //}
+    5506             : 
+    5507             : // | ------------- human-callable "goto" services ------------- |
+    5508             : 
+    5509             : /* //{ callbackGoto() */
+    5510             : 
+    5511          27 : bool ControlManager::callbackGoto(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res) {
+    5512             : 
+    5513          27 :   if (!is_initialized_) {
+    5514           0 :     res.message = "not initialized";
+    5515           0 :     res.success = false;
+    5516           0 :     return true;
+    5517             :   }
+    5518             : 
+    5519          81 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGoto");
+    5520          81 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGoto", scope_timer_logger_, scope_timer_enabled_);
+    5521             : 
+    5522          54 :   mrs_msgs::ReferenceStamped des_reference;
+    5523          27 :   des_reference.header.frame_id      = "";
+    5524          27 :   des_reference.header.stamp         = ros::Time(0);
+    5525          27 :   des_reference.reference.position.x = req.goal.at(REF_X);
+    5526          27 :   des_reference.reference.position.y = req.goal.at(REF_Y);
+    5527          27 :   des_reference.reference.position.z = req.goal.at(REF_Z);
+    5528          27 :   des_reference.reference.heading    = req.goal.at(REF_HEADING);
+    5529             : 
+    5530          54 :   auto [success, message] = setReference(des_reference);
+    5531             : 
+    5532          27 :   res.success = success;
+    5533          27 :   res.message = message;
+    5534             : 
+    5535          27 :   return true;
+    5536             : }
+    5537             : 
+    5538             : //}
+    5539             : 
+    5540             : /* //{ callbackGotoFcu() */
+    5541             : 
+    5542           1 : bool ControlManager::callbackGotoFcu(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res) {
+    5543             : 
+    5544           1 :   if (!is_initialized_) {
+    5545           0 :     res.message = "not initialized";
+    5546           0 :     res.success = false;
+    5547           0 :     return true;
+    5548             :   }
+    5549             : 
+    5550           3 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGotoFcu");
+    5551           3 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGotoFcu", scope_timer_logger_, scope_timer_enabled_);
+    5552             : 
+    5553           2 :   mrs_msgs::ReferenceStamped des_reference;
+    5554           1 :   des_reference.header.frame_id      = "fcu_untilted";
+    5555           1 :   des_reference.header.stamp         = ros::Time(0);
+    5556           1 :   des_reference.reference.position.x = req.goal.at(REF_X);
+    5557           1 :   des_reference.reference.position.y = req.goal.at(REF_Y);
+    5558           1 :   des_reference.reference.position.z = req.goal.at(REF_Z);
+    5559           1 :   des_reference.reference.heading    = req.goal.at(REF_HEADING);
+    5560             : 
+    5561           2 :   auto [success, message] = setReference(des_reference);
+    5562             : 
+    5563           1 :   res.success = success;
+    5564           1 :   res.message = message;
+    5565             : 
+    5566           1 :   return true;
+    5567             : }
+    5568             : 
+    5569             : //}
+    5570             : 
+    5571             : /* //{ callbackGotoRelative() */
+    5572             : 
+    5573          25 : bool ControlManager::callbackGotoRelative(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res) {
+    5574             : 
+    5575          25 :   if (!is_initialized_) {
+    5576           0 :     res.message = "not initialized";
+    5577           0 :     res.success = false;
+    5578           0 :     return true;
+    5579             :   }
+    5580             : 
+    5581          75 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGotoRelative");
+    5582          75 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGotoRelative", scope_timer_logger_, scope_timer_enabled_);
+    5583             : 
+    5584          50 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5585             : 
+    5586          25 :   if (!last_tracker_cmd) {
+    5587           0 :     res.message = "not flying";
+    5588           0 :     res.success = false;
+    5589           0 :     return true;
+    5590             :   }
+    5591             : 
+    5592          50 :   mrs_msgs::ReferenceStamped des_reference;
+    5593          25 :   des_reference.header.frame_id      = "";
+    5594          25 :   des_reference.header.stamp         = ros::Time(0);
+    5595          25 :   des_reference.reference.position.x = last_tracker_cmd->position.x + req.goal.at(REF_X);
+    5596          25 :   des_reference.reference.position.y = last_tracker_cmd->position.y + req.goal.at(REF_Y);
+    5597          25 :   des_reference.reference.position.z = last_tracker_cmd->position.z + req.goal.at(REF_Z);
+    5598          25 :   des_reference.reference.heading    = last_tracker_cmd->heading + req.goal.at(REF_HEADING);
+    5599             : 
+    5600          50 :   auto [success, message] = setReference(des_reference);
+    5601             : 
+    5602          25 :   res.success = success;
+    5603          25 :   res.message = message;
+    5604             : 
+    5605          25 :   return true;
+    5606             : }
+    5607             : 
+    5608             : //}
+    5609             : 
+    5610             : /* //{ callbackGotoAltitude() */
+    5611             : 
+    5612           2 : bool ControlManager::callbackGotoAltitude(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    5613             : 
+    5614           2 :   if (!is_initialized_) {
+    5615           0 :     res.message = "not initialized";
+    5616           0 :     res.success = false;
+    5617           0 :     return true;
+    5618             :   }
+    5619             : 
+    5620           6 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGotoAltitude");
+    5621           6 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGotoAltitude", scope_timer_logger_, scope_timer_enabled_);
+    5622             : 
+    5623           4 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5624             : 
+    5625           2 :   if (!last_tracker_cmd) {
+    5626           0 :     res.message = "not flying";
+    5627           0 :     res.success = false;
+    5628           0 :     return true;
+    5629             :   }
+    5630             : 
+    5631           4 :   mrs_msgs::ReferenceStamped des_reference;
+    5632           2 :   des_reference.header.frame_id      = "";
+    5633           2 :   des_reference.header.stamp         = ros::Time(0);
+    5634           2 :   des_reference.reference.position.x = last_tracker_cmd->position.x;
+    5635           2 :   des_reference.reference.position.y = last_tracker_cmd->position.y;
+    5636           2 :   des_reference.reference.position.z = req.goal;
+    5637           2 :   des_reference.reference.heading    = last_tracker_cmd->heading;
+    5638             : 
+    5639           4 :   auto [success, message] = setReference(des_reference);
+    5640             : 
+    5641           2 :   res.success = success;
+    5642           2 :   res.message = message;
+    5643             : 
+    5644           2 :   return true;
+    5645             : }
+    5646             : 
+    5647             : //}
+    5648             : 
+    5649             : /* //{ callbackSetHeading() */
+    5650             : 
+    5651           4 : bool ControlManager::callbackSetHeading(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    5652             : 
+    5653           4 :   if (!is_initialized_) {
+    5654           0 :     res.message = "not initialized";
+    5655           0 :     res.success = false;
+    5656           0 :     return true;
+    5657             :   }
+    5658             : 
+    5659          12 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackSetHeading");
+    5660          12 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackSetHeading", scope_timer_logger_, scope_timer_enabled_);
+    5661             : 
+    5662           8 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5663             : 
+    5664           4 :   if (!last_tracker_cmd) {
+    5665           0 :     res.message = "not flying";
+    5666           0 :     res.success = false;
+    5667           0 :     return true;
+    5668             :   }
+    5669             : 
+    5670           8 :   mrs_msgs::ReferenceStamped des_reference;
+    5671           4 :   des_reference.header.frame_id      = "";
+    5672           4 :   des_reference.header.stamp         = ros::Time(0);
+    5673           4 :   des_reference.reference.position.x = last_tracker_cmd->position.x;
+    5674           4 :   des_reference.reference.position.y = last_tracker_cmd->position.y;
+    5675           4 :   des_reference.reference.position.z = last_tracker_cmd->position.z;
+    5676           4 :   des_reference.reference.heading    = req.goal;
+    5677             : 
+    5678           8 :   auto [success, message] = setReference(des_reference);
+    5679             : 
+    5680           4 :   res.success = success;
+    5681           4 :   res.message = message;
+    5682             : 
+    5683           4 :   return true;
+    5684             : }
+    5685             : 
+    5686             : //}
+    5687             : 
+    5688             : /* //{ callbackSetHeadingRelative() */
+    5689             : 
+    5690           3 : bool ControlManager::callbackSetHeadingRelative(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    5691             : 
+    5692           3 :   if (!is_initialized_) {
+    5693           0 :     res.message = "not initialized";
+    5694           0 :     res.success = false;
+    5695           0 :     return true;
+    5696             :   }
+    5697             : 
+    5698           9 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackSetHeadingRelative");
+    5699           9 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackSetHeadingRelative", scope_timer_logger_, scope_timer_enabled_);
+    5700             : 
+    5701           6 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5702             : 
+    5703           3 :   if (!last_tracker_cmd) {
+    5704           0 :     res.message = "not flying";
+    5705           0 :     res.success = false;
+    5706           0 :     return true;
+    5707             :   }
+    5708             : 
+    5709           6 :   mrs_msgs::ReferenceStamped des_reference;
+    5710           3 :   des_reference.header.frame_id      = "";
+    5711           3 :   des_reference.header.stamp         = ros::Time(0);
+    5712           3 :   des_reference.reference.position.x = last_tracker_cmd->position.x;
+    5713           3 :   des_reference.reference.position.y = last_tracker_cmd->position.y;
+    5714           3 :   des_reference.reference.position.z = last_tracker_cmd->position.z;
+    5715           3 :   des_reference.reference.heading    = last_tracker_cmd->heading + req.goal;
+    5716             : 
+    5717           6 :   auto [success, message] = setReference(des_reference);
+    5718             : 
+    5719           3 :   res.success = success;
+    5720           3 :   res.message = message;
+    5721             : 
+    5722           3 :   return true;
+    5723             : }
+    5724             : 
+    5725             : //}
+    5726             : 
+    5727             : // --------------------------------------------------------------
+    5728             : // |                          routines                          |
+    5729             : // --------------------------------------------------------------
+    5730             : 
+    5731             : /* setReference() //{ */
+    5732             : 
+    5733          65 : std::tuple<bool, std::string> ControlManager::setReference(const mrs_msgs::ReferenceStamped reference_in) {
+    5734             : 
+    5735         130 :   std::stringstream ss;
+    5736             : 
+    5737          65 :   if (!callbacks_enabled_) {
+    5738           0 :     ss << "can not set the reference, the callbacks are disabled";
+    5739           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5740           0 :     return std::tuple(false, ss.str());
+    5741             :   }
+    5742             : 
+    5743          65 :   if (!validateReference(reference_in.reference, "ControlManager", "reference")) {
+    5744           0 :     ss << "incoming reference is not finite!!!";
+    5745           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5746           0 :     return std::tuple(false, ss.str());
+    5747             :   }
+    5748             : 
+    5749             :   // copy member variables
+    5750         130 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5751         130 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5752             : 
+    5753             :   // transform the reference to the current frame
+    5754         130 :   auto ret = transformer_->transformSingle(reference_in, uav_state.header.frame_id);
+    5755             : 
+    5756          65 :   if (!ret) {
+    5757             : 
+    5758           0 :     ss << "the reference could not be transformed";
+    5759           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5760           0 :     return std::tuple(false, ss.str());
+    5761             :   }
+    5762             : 
+    5763         130 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5764             : 
+    5765             :   // safety area check
+    5766          65 :   if (!isPointInSafetyArea3d(transformed_reference)) {
+    5767           0 :     ss << "failed to set the reference, the point is outside of the safety area!";
+    5768           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5769           0 :     return std::tuple(false, ss.str());
+    5770             :   }
+    5771             : 
+    5772          65 :   if (last_tracker_cmd) {
+    5773             : 
+    5774          65 :     mrs_msgs::ReferenceStamped from_point;
+    5775          65 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5776          65 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5777          65 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5778          65 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5779             : 
+    5780          65 :     if (!isPathToPointInSafetyArea3d(from_point, transformed_reference)) {
+    5781           0 :       ss << "failed to set the reference, the path is going outside the safety area!";
+    5782           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5783           0 :       return std::tuple(false, ss.str());
+    5784             :     }
+    5785             :   }
+    5786             : 
+    5787          65 :   mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;
+    5788             : 
+    5789             :   // prepare the message for current tracker
+    5790          65 :   mrs_msgs::ReferenceSrvRequest reference_request;
+    5791          65 :   reference_request.reference = transformed_reference.reference;
+    5792             : 
+    5793             :   {
+    5794         130 :     std::scoped_lock lock(mutex_tracker_list_);
+    5795             : 
+    5796          65 :     ROS_INFO("[ControlManager]: setting reference to x=%.2f, y=%.2f, z=%.2f, hdg=%.2f (expressed in '%s')", reference_request.reference.position.x,
+    5797             :              reference_request.reference.position.y, reference_request.reference.position.z, reference_request.reference.heading,
+    5798             :              transformed_reference.header.frame_id.c_str());
+    5799             : 
+    5800          65 :     tracker_response = tracker_list_.at(active_tracker_idx_)
+    5801          65 :                            ->setReference(mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(reference_request)));
+    5802             : 
+    5803          65 :     if (tracker_response != mrs_msgs::ReferenceSrvResponse::Ptr()) {
+    5804             : 
+    5805         130 :       return std::tuple(tracker_response->success, tracker_response->message);
+    5806             : 
+    5807             :     } else {
+    5808             : 
+    5809           0 :       ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'setReference()' function!";
+    5810           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: failed to set the reference: " << ss.str());
+    5811           0 :       return std::tuple(false, ss.str());
+    5812             :     }
+    5813             :   }
+    5814             : }
+    5815             : 
+    5816             : //}
+    5817             : 
+    5818             : /* setVelocityReference() //{ */
+    5819             : 
+    5820         809 : std::tuple<bool, std::string> ControlManager::setVelocityReference(const mrs_msgs::VelocityReferenceStamped& reference_in) {
+    5821             : 
+    5822        1618 :   std::stringstream ss;
+    5823             : 
+    5824         809 :   if (!callbacks_enabled_) {
+    5825           0 :     ss << "can not set the reference, the callbacks are disabled";
+    5826           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5827           0 :     return std::tuple(false, ss.str());
+    5828             :   }
+    5829             : 
+    5830         809 :   if (!validateVelocityReference(reference_in.reference, "ControlManager", "velocity_reference")) {
+    5831           0 :     ss << "velocity command is not valid!";
+    5832           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5833           0 :     return std::tuple(false, ss.str());
+    5834             :   }
+    5835             : 
+    5836             :   {
+    5837         809 :     std::scoped_lock lock(mutex_last_tracker_cmd_);
+    5838             : 
+    5839         809 :     if (!last_tracker_cmd_) {
+    5840           0 :       ss << "could not set velocity command, not flying!";
+    5841           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5842           0 :       return std::tuple(false, ss.str());
+    5843             :     }
+    5844             :   }
+    5845             : 
+    5846             :   // copy member variables
+    5847        1618 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5848        1618 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5849             : 
+    5850             :   // | -- transform the velocity reference to the current frame - |
+    5851             : 
+    5852        1618 :   mrs_msgs::VelocityReferenceStamped transformed_reference = reference_in;
+    5853             : 
+    5854        1618 :   auto ret = transformer_->getTransform(reference_in.header.frame_id, uav_state.header.frame_id, reference_in.header.stamp);
+    5855             : 
+    5856        1618 :   geometry_msgs::TransformStamped tf;
+    5857             : 
+    5858         809 :   if (!ret) {
+    5859           0 :     ss << "could not find tf from " << reference_in.header.frame_id << " to " << uav_state.header.frame_id;
+    5860           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5861           0 :     return std::tuple(false, ss.str());
+    5862             :   } else {
+    5863         809 :     tf = ret.value();
+    5864             :   }
+    5865             : 
+    5866             :   // transform the velocity
+    5867             :   {
+    5868         809 :     geometry_msgs::Vector3Stamped velocity;
+    5869         809 :     velocity.header   = reference_in.header;
+    5870         809 :     velocity.vector.x = reference_in.reference.velocity.x;
+    5871         809 :     velocity.vector.y = reference_in.reference.velocity.y;
+    5872         809 :     velocity.vector.z = reference_in.reference.velocity.z;
+    5873             : 
+    5874         809 :     auto ret = transformer_->transform(velocity, tf);
+    5875             : 
+    5876         809 :     if (!ret) {
+    5877             : 
+    5878           0 :       ss << "the velocity reference could not be transformed";
+    5879           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5880           0 :       return std::tuple(false, ss.str());
+    5881             : 
+    5882             :     } else {
+    5883         809 :       transformed_reference.reference.velocity.x = ret->vector.x;
+    5884         809 :       transformed_reference.reference.velocity.y = ret->vector.y;
+    5885         809 :       transformed_reference.reference.velocity.z = ret->vector.z;
+    5886             :     }
+    5887             :   }
+    5888             : 
+    5889             :   // transform the z and the heading
+    5890             :   {
+    5891         809 :     geometry_msgs::PoseStamped pose;
+    5892         809 :     pose.header           = reference_in.header;
+    5893         809 :     pose.pose.position.x  = 0;
+    5894         809 :     pose.pose.position.y  = 0;
+    5895         809 :     pose.pose.position.z  = reference_in.reference.altitude;
+    5896         809 :     pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, reference_in.reference.heading);
+    5897             : 
+    5898         809 :     auto ret = transformer_->transform(pose, tf);
+    5899             : 
+    5900         809 :     if (!ret) {
+    5901             : 
+    5902           0 :       ss << "the velocity reference could not be transformed";
+    5903           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5904           0 :       return std::tuple(false, ss.str());
+    5905             : 
+    5906             :     } else {
+    5907         809 :       transformed_reference.reference.altitude = ret->pose.position.z;
+    5908         809 :       transformed_reference.reference.heading  = mrs_lib::AttitudeConverter(ret->pose.orientation).getHeading();
+    5909             :     }
+    5910             :   }
+    5911             : 
+    5912             :   // the heading rate doees not need to be transformed
+    5913         809 :   transformed_reference.reference.heading_rate = reference_in.reference.heading_rate;
+    5914             : 
+    5915         809 :   transformed_reference.header.stamp    = tf.header.stamp;
+    5916         809 :   transformed_reference.header.frame_id = transformer_->frame_to(tf);
+    5917             : 
+    5918        1618 :   mrs_msgs::ReferenceStamped eqivalent_reference = velocityReferenceToReference(transformed_reference);
+    5919             : 
+    5920         809 :   ROS_DEBUG("[ControlManager]: equivalent reference: %.2f, %.2f, %.2f, %.2f", eqivalent_reference.reference.position.x,
+    5921             :             eqivalent_reference.reference.position.y, eqivalent_reference.reference.position.z, eqivalent_reference.reference.heading);
+    5922             : 
+    5923             :   // safety area check
+    5924         809 :   if (!isPointInSafetyArea3d(eqivalent_reference)) {
+    5925           0 :     ss << "failed to set the reference, the point is outside of the safety area!";
+    5926           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5927           0 :     return std::tuple(false, ss.str());
+    5928             :   }
+    5929             : 
+    5930         809 :   if (last_tracker_cmd) {
+    5931             : 
+    5932         809 :     mrs_msgs::ReferenceStamped from_point;
+    5933         809 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5934         809 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5935         809 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5936         809 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5937             : 
+    5938         809 :     if (!isPathToPointInSafetyArea3d(from_point, eqivalent_reference)) {
+    5939           0 :       ss << "failed to set the reference, the path is going outside the safety area!";
+    5940           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5941           0 :       return std::tuple(false, ss.str());
+    5942             :     }
+    5943             :   }
+    5944             : 
+    5945         809 :   mrs_msgs::VelocityReferenceSrvResponse::ConstPtr tracker_response;
+    5946             : 
+    5947             :   // prepare the message for current tracker
+    5948         809 :   mrs_msgs::VelocityReferenceSrvRequest reference_request;
+    5949         809 :   reference_request.reference = transformed_reference.reference;
+    5950             : 
+    5951             :   {
+    5952        1618 :     std::scoped_lock lock(mutex_tracker_list_);
+    5953             : 
+    5954             :     tracker_response =
+    5955         809 :         tracker_list_.at(active_tracker_idx_)
+    5956         809 :             ->setVelocityReference(mrs_msgs::VelocityReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::VelocityReferenceSrvRequest>(reference_request)));
+    5957             : 
+    5958         809 :     if (tracker_response != mrs_msgs::VelocityReferenceSrvResponse::Ptr()) {
+    5959             : 
+    5960        1618 :       return std::tuple(tracker_response->success, tracker_response->message);
+    5961             : 
+    5962             :     } else {
+    5963             : 
+    5964           0 :       ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'setVelocityReference()' function!";
+    5965           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: failed to set the velocity reference: " << ss.str());
+    5966           0 :       return std::tuple(false, ss.str());
+    5967             :     }
+    5968             :   }
+    5969             : }
+    5970             : 
+    5971             : //}
+    5972             : 
+    5973             : /* setTrajectoryReference() //{ */
+    5974             : 
+    5975           6 : std::tuple<bool, std::string, bool, std::vector<std::string>, std::vector<bool>, std::vector<std::string>> ControlManager::setTrajectoryReference(
+    5976             :     const mrs_msgs::TrajectoryReference trajectory_in) {
+    5977             : 
+    5978          12 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5979             : 
+    5980          12 :   std::stringstream ss;
+    5981             : 
+    5982           6 :   if (!callbacks_enabled_) {
+    5983           0 :     ss << "can not set the reference, the callbacks are disabled";
+    5984           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5985           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    5986             :   }
+    5987             : 
+    5988             :   /* validate the size and check for NaNs //{ */
+    5989             : 
+    5990             :   // check for the size 0, which is invalid
+    5991           6 :   if (trajectory_in.points.size() == 0) {
+    5992             : 
+    5993           0 :     ss << "can not load trajectory with size 0";
+    5994           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5995           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    5996             :   }
+    5997             : 
+    5998         700 :   for (int i = 0; i < int(trajectory_in.points.size()); i++) {
+    5999             : 
+    6000             :     // check the point for NaN/inf
+    6001         694 :     bool valid = validateReference(trajectory_in.points.at(i), "ControlManager", "trajectory_in.points");
+    6002             : 
+    6003         694 :     if (!valid) {
+    6004             : 
+    6005           0 :       ss << "trajectory contains NaNs/infs.";
+    6006           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6007           0 :       return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6008             :     }
+    6009             :   }
+    6010             : 
+    6011             :   //}
+    6012             : 
+    6013             :   /* publish the debugging topics of the original trajectory //{ */
+    6014             : 
+    6015             :   {
+    6016             : 
+    6017          12 :     geometry_msgs::PoseArray debug_trajectory_out;
+    6018           6 :     debug_trajectory_out.header = trajectory_in.header;
+    6019             : 
+    6020           6 :     debug_trajectory_out.header.frame_id = transformer_->resolveFrame(debug_trajectory_out.header.frame_id);
+    6021             : 
+    6022           6 :     if (debug_trajectory_out.header.stamp == ros::Time(0)) {
+    6023           4 :       debug_trajectory_out.header.stamp = ros::Time::now();
+    6024             :     }
+    6025             : 
+    6026         694 :     for (int i = 0; i < int(trajectory_in.points.size()) - 1; i++) {
+    6027             : 
+    6028         688 :       geometry_msgs::Pose new_pose;
+    6029             : 
+    6030         688 :       new_pose.position.x = trajectory_in.points.at(i).position.x;
+    6031         688 :       new_pose.position.y = trajectory_in.points.at(i).position.y;
+    6032         688 :       new_pose.position.z = trajectory_in.points.at(i).position.z;
+    6033             : 
+    6034         688 :       new_pose.orientation = mrs_lib::AttitudeConverter(0, 0, trajectory_in.points.at(i).heading);
+    6035             : 
+    6036         688 :       debug_trajectory_out.poses.push_back(new_pose);
+    6037             :     }
+    6038             : 
+    6039           6 :     pub_debug_original_trajectory_poses_.publish(debug_trajectory_out);
+    6040             : 
+    6041          12 :     visualization_msgs::MarkerArray msg_out;
+    6042             : 
+    6043          12 :     visualization_msgs::Marker marker;
+    6044             : 
+    6045           6 :     marker.header = trajectory_in.header;
+    6046             : 
+    6047           6 :     marker.header.frame_id = transformer_->resolveFrame(marker.header.frame_id);
+    6048             : 
+    6049           6 :     if (marker.header.frame_id == "") {
+    6050           0 :       marker.header.frame_id = uav_state.header.frame_id;
+    6051             :     }
+    6052             : 
+    6053           6 :     if (marker.header.stamp == ros::Time(0)) {
+    6054           4 :       marker.header.stamp = ros::Time::now();
+    6055             :     }
+    6056             : 
+    6057           6 :     marker.type             = visualization_msgs::Marker::LINE_LIST;
+    6058           6 :     marker.color.a          = 1;
+    6059           6 :     marker.scale.x          = 0.05;
+    6060           6 :     marker.color.r          = 0;
+    6061           6 :     marker.color.g          = 1;
+    6062           6 :     marker.color.b          = 0;
+    6063           6 :     marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    6064             : 
+    6065         694 :     for (int i = 0; i < int(trajectory_in.points.size()) - 1; i++) {
+    6066             : 
+    6067         688 :       geometry_msgs::Point point1;
+    6068             : 
+    6069         688 :       point1.x = trajectory_in.points.at(i).position.x;
+    6070         688 :       point1.y = trajectory_in.points.at(i).position.y;
+    6071         688 :       point1.z = trajectory_in.points.at(i).position.z;
+    6072             : 
+    6073         688 :       marker.points.push_back(point1);
+    6074             : 
+    6075         688 :       geometry_msgs::Point point2;
+    6076             : 
+    6077         688 :       point2.x = trajectory_in.points.at(i + 1).position.x;
+    6078         688 :       point2.y = trajectory_in.points.at(i + 1).position.y;
+    6079         688 :       point2.z = trajectory_in.points.at(i + 1).position.z;
+    6080             : 
+    6081         688 :       marker.points.push_back(point2);
+    6082             :     }
+    6083             : 
+    6084           6 :     msg_out.markers.push_back(marker);
+    6085             : 
+    6086           6 :     pub_debug_original_trajectory_markers_.publish(msg_out);
+    6087             :   }
+    6088             : 
+    6089             :   //}
+    6090             : 
+    6091          12 :   mrs_msgs::TrajectoryReference processed_trajectory = trajectory_in;
+    6092             : 
+    6093           6 :   int trajectory_size = int(processed_trajectory.points.size());
+    6094             : 
+    6095           6 :   bool trajectory_modified = false;
+    6096             : 
+    6097             :   /* safety area check //{ */
+    6098             : 
+    6099           6 :   if (use_safety_area_) {
+    6100             : 
+    6101           5 :     int last_valid_idx    = 0;
+    6102           5 :     int first_invalid_idx = -1;
+    6103             : 
+    6104           5 :     double min_z = getMinZ(processed_trajectory.header.frame_id);
+    6105           5 :     double max_z = getMaxZ(processed_trajectory.header.frame_id);
+    6106             : 
+    6107         678 :     for (int i = 0; i < trajectory_size; i++) {
+    6108             : 
+    6109         673 :       if (_snap_trajectory_to_safety_area_) {
+    6110             : 
+    6111             :         // saturate the trajectory to min and max Z
+    6112           0 :         if (processed_trajectory.points.at(i).position.z < min_z) {
+    6113             : 
+    6114           0 :           processed_trajectory.points.at(i).position.z = min_z;
+    6115           0 :           ROS_WARN_THROTTLE(1.0, "[ControlManager]: the trajectory violates the minimum Z!");
+    6116           0 :           trajectory_modified = true;
+    6117             :         }
+    6118             : 
+    6119           0 :         if (processed_trajectory.points.at(i).position.z > max_z) {
+    6120             : 
+    6121           0 :           processed_trajectory.points.at(i).position.z = max_z;
+    6122           0 :           ROS_WARN_THROTTLE(1.0, "[ControlManager]: the trajectory violates the maximum Z!");
+    6123           0 :           trajectory_modified = true;
+    6124             :         }
+    6125             :       }
+    6126             : 
+    6127             :       // check the point against the safety area
+    6128         673 :       mrs_msgs::ReferenceStamped des_reference;
+    6129         673 :       des_reference.header    = processed_trajectory.header;
+    6130         673 :       des_reference.reference = processed_trajectory.points.at(i);
+    6131             : 
+    6132         673 :       if (!isPointInSafetyArea3d(des_reference)) {
+    6133             : 
+    6134           0 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: the trajectory contains points outside of the safety area!");
+    6135           0 :         trajectory_modified = true;
+    6136             : 
+    6137             :         // the first invalid point
+    6138           0 :         if (first_invalid_idx == -1) {
+    6139             : 
+    6140           0 :           first_invalid_idx = i;
+    6141             : 
+    6142           0 :           last_valid_idx = i - 1;
+    6143             :         }
+    6144             : 
+    6145             :         // the point is ok
+    6146             :       } else {
+    6147             : 
+    6148             :         // we found a point, which is ok, after finding a point which was not ok
+    6149         673 :         if (first_invalid_idx != -1) {
+    6150             : 
+    6151             :           // special case, we had no valid point so far
+    6152           0 :           if (last_valid_idx == -1) {
+    6153             : 
+    6154           0 :             ss << "the trajectory starts outside of the safety area!";
+    6155           0 :             ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6156           0 :             return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6157             : 
+    6158             :             // we have a valid point in the past
+    6159             :           } else {
+    6160             : 
+    6161           0 :             if (!_snap_trajectory_to_safety_area_) {
+    6162           0 :               break;
+    6163             :             }
+    6164             : 
+    6165           0 :             bool interpolation_success = true;
+    6166             : 
+    6167             :             // iterpolate between the last valid point and this new valid point
+    6168           0 :             double angle = atan2((processed_trajectory.points.at(i).position.y - processed_trajectory.points.at(last_valid_idx).position.y),
+    6169           0 :                                  (processed_trajectory.points.at(i).position.x - processed_trajectory.points.at(last_valid_idx).position.x));
+    6170             : 
+    6171           0 :             double dist_two_points = mrs_lib::geometry::dist(
+    6172           0 :                 vec2_t(processed_trajectory.points.at(i).position.x, processed_trajectory.points.at(i).position.y),
+    6173           0 :                 vec2_t(processed_trajectory.points.at(last_valid_idx).position.x, processed_trajectory.points.at(last_valid_idx).position.y));
+    6174           0 :             double step = dist_two_points / (i - last_valid_idx);
+    6175             : 
+    6176           0 :             for (int j = last_valid_idx; j < i; j++) {
+    6177             : 
+    6178           0 :               mrs_msgs::ReferenceStamped temp_point;
+    6179           0 :               temp_point.header.frame_id      = processed_trajectory.header.frame_id;
+    6180           0 :               temp_point.reference.position.x = processed_trajectory.points.at(last_valid_idx).position.x + (j - last_valid_idx) * cos(angle) * step;
+    6181           0 :               temp_point.reference.position.y = processed_trajectory.points.at(last_valid_idx).position.y + (j - last_valid_idx) * sin(angle) * step;
+    6182             : 
+    6183           0 :               if (!isPointInSafetyArea2d(temp_point)) {
+    6184             : 
+    6185           0 :                 interpolation_success = false;
+    6186           0 :                 break;
+    6187             : 
+    6188             :               } else {
+    6189             : 
+    6190           0 :                 processed_trajectory.points.at(j).position.x = temp_point.reference.position.x;
+    6191           0 :                 processed_trajectory.points.at(j).position.y = temp_point.reference.position.y;
+    6192             :               }
+    6193             :             }
+    6194             : 
+    6195           0 :             if (!interpolation_success) {
+    6196           0 :               break;
+    6197             :             }
+    6198             :           }
+    6199             : 
+    6200           0 :           first_invalid_idx = -1;
+    6201             :         }
+    6202             :       }
+    6203             :     }
+    6204             : 
+    6205             :     // special case, the trajectory does not end with a valid point
+    6206           5 :     if (first_invalid_idx != -1) {
+    6207             : 
+    6208             :       // super special case, the whole trajectory is invalid
+    6209           0 :       if (first_invalid_idx == 0) {
+    6210             : 
+    6211           0 :         ss << "the whole trajectory is outside of the safety area!";
+    6212           0 :         ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6213           0 :         return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6214             : 
+    6215             :         // there is a good portion of the trajectory in the beginning
+    6216             :       } else {
+    6217             : 
+    6218           0 :         trajectory_size = last_valid_idx + 1;
+    6219           0 :         processed_trajectory.points.resize(trajectory_size);
+    6220           0 :         trajectory_modified = true;
+    6221             :       }
+    6222             :     }
+    6223             :   }
+    6224             : 
+    6225           6 :   if (trajectory_size == 0) {
+    6226             : 
+    6227           0 :     ss << "the trajectory happened to be empty after all the checks! This message should not appear!";
+    6228           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6229           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6230             :   }
+    6231             : 
+    6232             :   //}
+    6233             : 
+    6234             :   /* transform the trajectory to the current control frame //{ */
+    6235             : 
+    6236           6 :   std::optional<geometry_msgs::TransformStamped> tf_traj_state;
+    6237             : 
+    6238           6 :   if (processed_trajectory.header.stamp > ros::Time::now()) {
+    6239           0 :     tf_traj_state = transformer_->getTransform(processed_trajectory.header.frame_id, "", processed_trajectory.header.stamp);
+    6240             :   } else {
+    6241           6 :     tf_traj_state = transformer_->getTransform(processed_trajectory.header.frame_id, "", uav_state_.header.stamp);
+    6242             :   }
+    6243             : 
+    6244           6 :   if (!tf_traj_state) {
+    6245           0 :     ss << "could not create TF transformer for the trajectory";
+    6246           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6247           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6248             :   }
+    6249             : 
+    6250           6 :   processed_trajectory.header.frame_id = transformer_->frame_to(*tf_traj_state);
+    6251             : 
+    6252         700 :   for (int i = 0; i < trajectory_size; i++) {
+    6253             : 
+    6254         694 :     mrs_msgs::ReferenceStamped trajectory_point;
+    6255         694 :     trajectory_point.header    = processed_trajectory.header;
+    6256         694 :     trajectory_point.reference = processed_trajectory.points.at(i);
+    6257             : 
+    6258         694 :     auto ret = transformer_->transform(trajectory_point, *tf_traj_state);
+    6259             : 
+    6260         694 :     if (!ret) {
+    6261             : 
+    6262           0 :       ss << "trajectory cannnot be transformed";
+    6263           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6264           0 :       return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6265             : 
+    6266             :     } else {
+    6267             : 
+    6268             :       // transform the points in the trajectory to the current frame
+    6269         694 :       processed_trajectory.points.at(i) = ret.value().reference;
+    6270             :     }
+    6271             :   }
+    6272             : 
+    6273             :   //}
+    6274             : 
+    6275           6 :   mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr response;
+    6276          12 :   mrs_msgs::TrajectoryReferenceSrvRequest            request;
+    6277             : 
+    6278             :   // check for empty trajectory
+    6279           6 :   if (processed_trajectory.points.size() == 0) {
+    6280           0 :     ss << "reference trajectory was processing and it is now empty, this should not happen!";
+    6281           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6282           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6283             :   }
+    6284             : 
+    6285             :   // prepare the message for current tracker
+    6286           6 :   request.trajectory = processed_trajectory;
+    6287             : 
+    6288             :   bool                     success;
+    6289          12 :   std::string              message;
+    6290             :   bool                     modified;
+    6291          12 :   std::vector<std::string> tracker_names;
+    6292          12 :   std::vector<bool>        tracker_successes;
+    6293          12 :   std::vector<std::string> tracker_messages;
+    6294             : 
+    6295             :   {
+    6296          12 :     std::scoped_lock lock(mutex_tracker_list_);
+    6297             : 
+    6298             :     // set the trajectory to the currently active tracker
+    6299             :     response =
+    6300           6 :         tracker_list_.at(active_tracker_idx_)
+    6301           6 :             ->setTrajectoryReference(mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::TrajectoryReferenceSrvRequest>(request)));
+    6302             : 
+    6303           6 :     tracker_names.push_back(_tracker_names_.at(active_tracker_idx_));
+    6304             : 
+    6305           6 :     if (response != mrs_msgs::TrajectoryReferenceSrvResponse::Ptr()) {
+    6306             : 
+    6307           5 :       success  = response->success;
+    6308           5 :       message  = response->message;
+    6309           5 :       modified = response->modified || trajectory_modified;
+    6310           5 :       tracker_successes.push_back(response->success);
+    6311           5 :       tracker_messages.push_back(response->message);
+    6312             : 
+    6313             :     } else {
+    6314             : 
+    6315           1 :       ss << "the active tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'setTrajectoryReference()' function!";
+    6316           1 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6317             : 
+    6318           1 :       success  = true;
+    6319           1 :       message  = ss.str();
+    6320           1 :       modified = false;
+    6321           1 :       tracker_successes.push_back(false);
+    6322           1 :       tracker_messages.push_back(ss.str());
+    6323             :     }
+    6324             : 
+    6325             :     // set the trajectory to the non-active trackers
+    6326          42 :     for (size_t i = 0; i < tracker_list_.size(); i++) {
+    6327             : 
+    6328          36 :       if (i != active_tracker_idx_) {
+    6329             : 
+    6330          30 :         tracker_names.push_back(_tracker_names_.at(i));
+    6331             : 
+    6332          90 :         response = tracker_list_.at(i)->setTrajectoryReference(
+    6333          90 :             mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::TrajectoryReferenceSrvRequest>(request)));
+    6334             : 
+    6335          30 :         if (response != mrs_msgs::TrajectoryReferenceSrvResponse::Ptr()) {
+    6336             : 
+    6337           1 :           tracker_successes.push_back(response->success);
+    6338           1 :           tracker_messages.push_back(response->message);
+    6339             : 
+    6340           1 :           if (response->success) {
+    6341           2 :             std::stringstream ss;
+    6342           1 :             ss << "trajectory loaded to non-active tracker '" << _tracker_names_.at(i);
+    6343           1 :             ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6344             :           }
+    6345             : 
+    6346             :         } else {
+    6347             : 
+    6348          29 :           std::stringstream ss;
+    6349          29 :           ss << "the tracker \"" << _tracker_names_.at(i) << "\" does not implement setTrajectoryReference()";
+    6350          29 :           tracker_successes.push_back(false);
+    6351          29 :           tracker_messages.push_back(ss.str());
+    6352             :         }
+    6353             :       }
+    6354             :     }
+    6355             :   }
+    6356             : 
+    6357           6 :   return std::tuple(success, message, modified, tracker_names, tracker_successes, tracker_messages);
+    6358             : }
+    6359             : 
+    6360             : //}
+    6361             : 
+    6362             : /* isOffboard() //{ */
+    6363             : 
+    6364          18 : bool ControlManager::isOffboard(void) {
+    6365             : 
+    6366          18 :   if (!sh_hw_api_status_.hasMsg()) {
+    6367           0 :     return false;
+    6368             :   }
+    6369             : 
+    6370          18 :   mrs_msgs::HwApiStatusConstPtr hw_api_status = sh_hw_api_status_.getMsg();
+    6371             : 
+    6372          18 :   return hw_api_status->connected && hw_api_status->offboard;
+    6373             : }
+    6374             : 
+    6375             : //}
+    6376             : 
+    6377             : /* setCallbacks() //{ */
+    6378             : 
+    6379         102 : void ControlManager::setCallbacks(bool in) {
+    6380             : 
+    6381         102 :   callbacks_enabled_ = in;
+    6382             : 
+    6383         102 :   std_srvs::SetBoolRequest req_enable_callbacks;
+    6384         102 :   req_enable_callbacks.data = callbacks_enabled_;
+    6385             : 
+    6386             :   {
+    6387         204 :     std::scoped_lock lock(mutex_tracker_list_);
+    6388             : 
+    6389             :     // set callbacks to all trackers
+    6390         714 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    6391         612 :       tracker_list_.at(i)->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    6392             :     }
+    6393             :   }
+    6394         102 : }
+    6395             : 
+    6396             : //}
+    6397             : 
+    6398             : /* publishDiagnostics() //{ */
+    6399             : 
+    6400       19545 : void ControlManager::publishDiagnostics(void) {
+    6401             : 
+    6402       19545 :   if (!is_initialized_) {
+    6403           0 :     return;
+    6404             :   }
+    6405             : 
+    6406       58635 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("publishDiagnostics");
+    6407       58635 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::publishDiagnostics", scope_timer_logger_, scope_timer_enabled_);
+    6408             : 
+    6409       39090 :   std::scoped_lock lock(mutex_diagnostics_);
+    6410             : 
+    6411       39090 :   mrs_msgs::ControlManagerDiagnostics diagnostics_msg;
+    6412             : 
+    6413       19545 :   diagnostics_msg.stamp    = ros::Time::now();
+    6414       19545 :   diagnostics_msg.uav_name = _uav_name_;
+    6415             : 
+    6416       19545 :   diagnostics_msg.desired_uav_state_rate = desired_uav_state_rate_;
+    6417             : 
+    6418       19545 :   diagnostics_msg.output_enabled = output_enabled_;
+    6419             : 
+    6420       19545 :   diagnostics_msg.joystick_active = rc_goto_active_;
+    6421             : 
+    6422             :   {
+    6423       19545 :     std::scoped_lock lock(mutex_tracker_list_, mutex_controller_list_);
+    6424             : 
+    6425       19545 :     diagnostics_msg.flying_normally = isFlyingNormally();
+    6426             :   }
+    6427             : 
+    6428       19545 :   diagnostics_msg.bumper_active = bumper_repulsing_;
+    6429             : 
+    6430             :   // | ----------------- fill the tracker status ---------------- |
+    6431             : 
+    6432             :   {
+    6433       39090 :     std::scoped_lock lock(mutex_tracker_list_);
+    6434             : 
+    6435       19545 :     mrs_msgs::TrackerStatus tracker_status;
+    6436             : 
+    6437       19545 :     diagnostics_msg.active_tracker = _tracker_names_.at(active_tracker_idx_);
+    6438       19545 :     diagnostics_msg.tracker_status = tracker_list_.at(active_tracker_idx_)->getStatus();
+    6439             :   }
+    6440             : 
+    6441             :   // | --------------- fill the controller status --------------- |
+    6442             : 
+    6443             :   {
+    6444       39090 :     std::scoped_lock lock(mutex_controller_list_);
+    6445             : 
+    6446       19545 :     mrs_msgs::ControllerStatus controller_status;
+    6447             : 
+    6448       19545 :     diagnostics_msg.active_controller = _controller_names_.at(active_controller_idx_);
+    6449       19545 :     diagnostics_msg.controller_status = controller_list_.at(active_controller_idx_)->getStatus();
+    6450             :   }
+    6451             : 
+    6452             :   // | ------------ fill in the available controllers ----------- |
+    6453             : 
+    6454      117270 :   for (int i = 0; i < int(_controller_names_.size()); i++) {
+    6455       97725 :     if ((_controller_names_.at(i) != _failsafe_controller_name_) && (_controller_names_.at(i) != _eland_controller_name_)) {
+    6456       58635 :       diagnostics_msg.available_controllers.push_back(_controller_names_.at(i));
+    6457       58635 :       diagnostics_msg.human_switchable_controllers.push_back(controllers_.at(_controller_names_.at(i)).human_switchable);
+    6458             :     }
+    6459             :   }
+    6460             : 
+    6461             :   // | ------------- fill in the available trackers ------------- |
+    6462             : 
+    6463      137035 :   for (int i = 0; i < int(_tracker_names_.size()); i++) {
+    6464      117490 :     if (_tracker_names_.at(i) != _null_tracker_name_) {
+    6465       97945 :       diagnostics_msg.available_trackers.push_back(_tracker_names_.at(i));
+    6466       97945 :       diagnostics_msg.human_switchable_trackers.push_back(trackers_.at(_tracker_names_.at(i)).human_switchable);
+    6467             :     }
+    6468             :   }
+    6469             : 
+    6470             :   // | ------------------------- publish ------------------------ |
+    6471             : 
+    6472       19545 :   ph_diagnostics_.publish(diagnostics_msg);
+    6473             : }
+    6474             : 
+    6475             : //}
+    6476             : 
+    6477             : /* setConstraintsToTrackers() //{ */
+    6478             : 
+    6479         387 : void ControlManager::setConstraintsToTrackers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6480             : 
+    6481        1161 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("setConstraintsToTrackers");
+    6482        1161 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::setConstraintsToTrackers", scope_timer_logger_, scope_timer_enabled_);
+    6483             : 
+    6484         387 :   mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr response;
+    6485             : 
+    6486             :   {
+    6487         774 :     std::scoped_lock lock(mutex_tracker_list_);
+    6488             : 
+    6489             :     // for each tracker
+    6490        2712 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    6491             : 
+    6492             :       // if it is the active one, update and retrieve the command
+    6493        6975 :       response = tracker_list_.at(i)->setConstraints(
+    6494        6975 :           mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr(std::make_unique<mrs_msgs::DynamicsConstraintsSrvRequest>(constraints)));
+    6495             :     }
+    6496             :   }
+    6497         387 : }
+    6498             : 
+    6499             : //}
+    6500             : 
+    6501             : /* setConstraintsToControllers() //{ */
+    6502             : 
+    6503         431 : void ControlManager::setConstraintsToControllers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6504             : 
+    6505        1293 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("setConstraintsToControllers");
+    6506        1293 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::setConstraintsToControllers", scope_timer_logger_, scope_timer_enabled_);
+    6507             : 
+    6508         431 :   mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr response;
+    6509             : 
+    6510             :   {
+    6511         862 :     std::scoped_lock lock(mutex_controller_list_);
+    6512             : 
+    6513             :     // for each controller
+    6514        2586 :     for (int i = 0; i < int(controller_list_.size()); i++) {
+    6515             : 
+    6516             :       // if it is the active one, update and retrieve the command
+    6517        6465 :       response = controller_list_.at(i)->setConstraints(
+    6518        6465 :           mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr(std::make_unique<mrs_msgs::DynamicsConstraintsSrvRequest>(constraints)));
+    6519             :     }
+    6520             :   }
+    6521         431 : }
+    6522             : 
+    6523             : //}
+    6524             : 
+    6525             : /* setConstraints() //{ */
+    6526             : 
+    6527         109 : void ControlManager::setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6528             : 
+    6529         327 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("setConstraints");
+    6530         327 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::setConstraints", scope_timer_logger_, scope_timer_enabled_);
+    6531             : 
+    6532         109 :   mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr response;
+    6533             : 
+    6534         109 :   setConstraintsToTrackers(constraints);
+    6535             : 
+    6536         109 :   setConstraintsToControllers(constraints);
+    6537         109 : }
+    6538             : 
+    6539             : //}
+    6540             : 
+    6541             : /* enforceControllerConstraints() //{ */
+    6542             : 
+    6543      141539 : std::optional<mrs_msgs::DynamicsConstraintsSrvRequest> ControlManager::enforceControllersConstraints(
+    6544             :     const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6545             : 
+    6546             :   // copy member variables
+    6547      283078 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    6548             : 
+    6549      141539 :   if (!last_control_output.control_output || !last_control_output.diagnostics.controller_enforcing_constraints) {
+    6550      121640 :     return {};
+    6551             :   }
+    6552             : 
+    6553       19899 :   bool enforcing = false;
+    6554             : 
+    6555       19899 :   auto constraints_out = constraints;
+    6556             : 
+    6557       39798 :   std::scoped_lock lock(mutex_tracker_list_);
+    6558             : 
+    6559             :   // enforce horizontal speed
+    6560       19899 :   if (last_control_output.diagnostics.horizontal_speed_constraint < constraints.constraints.horizontal_speed) {
+    6561       14549 :     constraints_out.constraints.horizontal_speed = last_control_output.diagnostics.horizontal_speed_constraint;
+    6562             : 
+    6563       14549 :     enforcing = true;
+    6564             :   }
+    6565             : 
+    6566             :   // enforce horizontal acceleration
+    6567       19899 :   if (last_control_output.diagnostics.horizontal_acc_constraint < constraints.constraints.horizontal_acceleration) {
+    6568       18205 :     constraints_out.constraints.horizontal_acceleration = last_control_output.diagnostics.horizontal_acc_constraint;
+    6569             : 
+    6570       18205 :     enforcing = true;
+    6571             :   }
+    6572             : 
+    6573             :   // enforce vertical ascending speed
+    6574       19899 :   if (last_control_output.diagnostics.vertical_asc_speed_constraint < constraints.constraints.vertical_ascending_speed) {
+    6575       14549 :     constraints_out.constraints.vertical_ascending_speed = last_control_output.diagnostics.vertical_asc_speed_constraint;
+    6576             : 
+    6577       14549 :     enforcing = true;
+    6578             :   }
+    6579             : 
+    6580             :   // enforce vertical ascending acceleration
+    6581       19899 :   if (last_control_output.diagnostics.vertical_asc_acc_constraint < constraints.constraints.vertical_ascending_acceleration) {
+    6582           0 :     constraints_out.constraints.vertical_ascending_acceleration = last_control_output.diagnostics.vertical_asc_acc_constraint;
+    6583             : 
+    6584           0 :     enforcing = true;
+    6585             :   }
+    6586             : 
+    6587             :   // enforce vertical descending speed
+    6588       19899 :   if (last_control_output.diagnostics.vertical_desc_speed_constraint < constraints.constraints.vertical_descending_speed) {
+    6589       14549 :     constraints_out.constraints.vertical_descending_speed = last_control_output.diagnostics.vertical_desc_speed_constraint;
+    6590             : 
+    6591       14549 :     enforcing = true;
+    6592             :   }
+    6593             : 
+    6594             :   // enforce vertical descending acceleration
+    6595       19899 :   if (last_control_output.diagnostics.vertical_desc_acc_constraint < constraints.constraints.vertical_descending_acceleration) {
+    6596           0 :     constraints_out.constraints.vertical_descending_acceleration = last_control_output.diagnostics.vertical_desc_acc_constraint;
+    6597             : 
+    6598           0 :     enforcing = true;
+    6599             :   }
+    6600             : 
+    6601       19899 :   if (enforcing) {
+    6602       18205 :     return {constraints_out};
+    6603             :   } else {
+    6604        1694 :     return {};
+    6605             :   }
+    6606             : }
+    6607             : 
+    6608             : //}
+    6609             : 
+    6610             : /* isFlyingNormally() //{ */
+    6611             : 
+    6612       19826 : bool ControlManager::isFlyingNormally(void) {
+    6613             : 
+    6614       17157 :   return callbacks_enabled_ && (output_enabled_) && (offboard_mode_) && (armed_) &&
+    6615       10715 :          (((active_tracker_idx_ != _ehover_tracker_idx_) && (active_controller_idx_ != _eland_controller_idx_) &&
+    6616       10715 :            (active_controller_idx_ != _failsafe_controller_idx_)) ||
+    6617       39301 :           _controller_names_.size() == 1) &&
+    6618       28223 :          (((active_tracker_idx_ != _null_tracker_idx_) && (active_tracker_idx_ != _landoff_tracker_idx_)) || _tracker_names_.size() == 1);
+    6619             : }
+    6620             : 
+    6621             : //}
+    6622             : 
+    6623             : /* //{ getMass() */
+    6624             : 
+    6625         560 : double ControlManager::getMass(void) {
+    6626             : 
+    6627        1120 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    6628             : 
+    6629         560 :   if (last_control_output.diagnostics.mass_estimator) {
+    6630          13 :     return _uav_mass_ + last_control_output.diagnostics.mass_difference;
+    6631             :   } else {
+    6632         547 :     return _uav_mass_;
+    6633             :   }
+    6634             : }
+    6635             : 
+    6636             : //}
+    6637             : 
+    6638             : /* loadConfigFile() //{ */
+    6639             : 
+    6640           0 : bool ControlManager::loadConfigFile(const std::string& file_path, const std::string ns) {
+    6641             : 
+    6642           0 :   const std::string name_space = nh_.getNamespace() + "/" + ns;
+    6643             : 
+    6644           0 :   ROS_INFO("[ControlManager]: loading '%s' under the namespace '%s'", file_path.c_str(), name_space.c_str());
+    6645             : 
+    6646             :   // load the user-requested file
+    6647             :   {
+    6648           0 :     std::string command = "rosparam load " + file_path + " " + name_space;
+    6649           0 :     int         result  = std::system(command.c_str());
+    6650             : 
+    6651           0 :     if (result != 0) {
+    6652           0 :       ROS_ERROR("[ControlManager]: failed to load '%s'", file_path.c_str());
+    6653           0 :       return false;
+    6654             :     }
+    6655             :   }
+    6656             : 
+    6657             :   // load the platform config
+    6658           0 :   if (_platform_config_ != "") {
+    6659           0 :     std::string command = "rosparam load " + _platform_config_ + " " + name_space;
+    6660           0 :     int         result  = std::system(command.c_str());
+    6661             : 
+    6662           0 :     if (result != 0) {
+    6663           0 :       ROS_ERROR("[ControlManager]: failed to load the platform config file '%s'", _platform_config_.c_str());
+    6664           0 :       return false;
+    6665             :     }
+    6666             :   }
+    6667             : 
+    6668             :   // load the custom config
+    6669           0 :   if (_custom_config_ != "") {
+    6670           0 :     std::string command = "rosparam load " + _custom_config_ + " " + name_space;
+    6671           0 :     int         result  = std::system(command.c_str());
+    6672             : 
+    6673           0 :     if (result != 0) {
+    6674           0 :       ROS_ERROR("[ControlManager]: failed to load the custom config file '%s'", _custom_config_.c_str());
+    6675           0 :       return false;
+    6676             :     }
+    6677             :   }
+    6678             : 
+    6679           0 :   return true;
+    6680             : }
+    6681             : 
+    6682             : //}
+    6683             : 
+    6684             : // | ----------------------- safety area ---------------------- |
+    6685             : 
+    6686             : /* //{ isPointInSafetyArea3d() */
+    6687             : 
+    6688        1842 : bool ControlManager::isPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& point) {
+    6689             : 
+    6690        1842 :   if (!use_safety_area_) {
+    6691         735 :     return true;
+    6692             :   }
+    6693             : 
+    6694        2214 :   auto tfed_horizontal = transformer_->transformSingle(point, _safety_area_horizontal_frame_);
+    6695             : 
+    6696        1107 :   if (!tfed_horizontal) {
+    6697           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform the point to the safety area horizontal frame");
+    6698           0 :     return false;
+    6699             :   }
+    6700             : 
+    6701        1107 :   if (!safety_zone_->isPointValid(tfed_horizontal->reference.position.x, tfed_horizontal->reference.position.y)) {
+    6702           3 :     return false;
+    6703             :   }
+    6704             : 
+    6705        1104 :   if (point.reference.position.z < getMinZ(point.header.frame_id) || point.reference.position.z > getMaxZ(point.header.frame_id)) {
+    6706           3 :     return false;
+    6707             :   }
+    6708             : 
+    6709        1101 :   return true;
+    6710             : }
+    6711             : 
+    6712             : //}
+    6713             : 
+    6714             : /* //{ isPointInSafetyArea2d() */
+    6715             : 
+    6716       10181 : bool ControlManager::isPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& point) {
+    6717             : 
+    6718       10181 :   if (!use_safety_area_) {
+    6719         588 :     return true;
+    6720             :   }
+    6721             : 
+    6722       19186 :   auto tfed_horizontal = transformer_->transformSingle(point, _safety_area_horizontal_frame_);
+    6723             : 
+    6724        9593 :   if (!tfed_horizontal) {
+    6725           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform the point to the safety area horizontal frame");
+    6726           0 :     return false;
+    6727             :   }
+    6728             : 
+    6729        9593 :   if (!safety_zone_->isPointValid(tfed_horizontal->reference.position.x, tfed_horizontal->reference.position.y)) {
+    6730          76 :     return false;
+    6731             :   }
+    6732             : 
+    6733        9517 :   return true;
+    6734             : }
+    6735             : 
+    6736             : //}
+    6737             : 
+    6738             : /* //{ isPathToPointInSafetyArea3d() */
+    6739             : 
+    6740         879 : bool ControlManager::isPathToPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& start, const mrs_msgs::ReferenceStamped& end) {
+    6741             : 
+    6742         879 :   if (!use_safety_area_) {
+    6743         735 :     return true;
+    6744             :   }
+    6745             : 
+    6746         144 :   if (!isPointInSafetyArea3d(start) || !isPointInSafetyArea3d(end)) {
+    6747           2 :     return false;
+    6748             :   }
+    6749             : 
+    6750         284 :   mrs_msgs::ReferenceStamped start_transformed, end_transformed;
+    6751             : 
+    6752             :   {
+    6753         142 :     auto ret = transformer_->transformSingle(start, _safety_area_horizontal_frame_);
+    6754             : 
+    6755         142 :     if (!ret) {
+    6756             : 
+    6757           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6758             : 
+    6759           0 :       return false;
+    6760             :     }
+    6761             : 
+    6762         142 :     start_transformed = ret.value();
+    6763             :   }
+    6764             : 
+    6765             :   {
+    6766         142 :     auto ret = transformer_->transformSingle(end, _safety_area_horizontal_frame_);
+    6767             : 
+    6768         142 :     if (!ret) {
+    6769             : 
+    6770           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6771             : 
+    6772           0 :       return false;
+    6773             :     }
+    6774             : 
+    6775         142 :     end_transformed = ret.value();
+    6776             :   }
+    6777             : 
+    6778         142 :   return safety_zone_->isPathValid(start_transformed.reference.position.x, start_transformed.reference.position.y, end_transformed.reference.position.x,
+    6779         142 :                                    end_transformed.reference.position.y);
+    6780             : }
+    6781             : 
+    6782             : //}
+    6783             : 
+    6784             : /* //{ isPathToPointInSafetyArea2d() */
+    6785             : 
+    6786           0 : bool ControlManager::isPathToPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& start, const mrs_msgs::ReferenceStamped& end) {
+    6787             : 
+    6788           0 :   if (!use_safety_area_) {
+    6789           0 :     return true;
+    6790             :   }
+    6791             : 
+    6792           0 :   mrs_msgs::ReferenceStamped start_transformed, end_transformed;
+    6793             : 
+    6794           0 :   if (!isPointInSafetyArea2d(start) || !isPointInSafetyArea2d(end)) {
+    6795           0 :     return false;
+    6796             :   }
+    6797             : 
+    6798             :   {
+    6799           0 :     auto ret = transformer_->transformSingle(start, _safety_area_horizontal_frame_);
+    6800             : 
+    6801           0 :     if (!ret) {
+    6802             : 
+    6803           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6804             : 
+    6805           0 :       return false;
+    6806             :     }
+    6807             : 
+    6808           0 :     start_transformed = ret.value();
+    6809             :   }
+    6810             : 
+    6811             :   {
+    6812           0 :     auto ret = transformer_->transformSingle(end, _safety_area_horizontal_frame_);
+    6813             : 
+    6814           0 :     if (!ret) {
+    6815             : 
+    6816           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6817             : 
+    6818           0 :       return false;
+    6819             :     }
+    6820             : 
+    6821           0 :     end_transformed = ret.value();
+    6822             :   }
+    6823             : 
+    6824           0 :   return safety_zone_->isPathValid(start_transformed.reference.position.x, start_transformed.reference.position.y, end_transformed.reference.position.x,
+    6825           0 :                                    end_transformed.reference.position.y);
+    6826             : }
+    6827             : 
+    6828             : //}
+    6829             : 
+    6830             : /* //{ getMaxZ() */
+    6831             : 
+    6832       13910 : double ControlManager::getMaxZ(const std::string& frame_id) {
+    6833             : 
+    6834             :   // | ------- first, calculate max_z from the safety area ------ |
+    6835             : 
+    6836       13910 :   double safety_area_max_z = std::numeric_limits<float>::max();
+    6837             : 
+    6838             :   {
+    6839             : 
+    6840       27820 :     geometry_msgs::PointStamped point;
+    6841             : 
+    6842       13910 :     point.header.frame_id = _safety_area_vertical_frame_;
+    6843       13910 :     point.point.x         = 0;
+    6844       13910 :     point.point.y         = 0;
+    6845       13910 :     point.point.z         = _safety_area_max_z_;
+    6846             : 
+    6847       13910 :     auto ret = transformer_->transformSingle(point, frame_id);
+    6848             : 
+    6849       13910 :     if (!ret) {
+    6850           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform safety area's max_z to '%s'", frame_id.c_str());
+    6851             :     }
+    6852             : 
+    6853       13910 :     safety_area_max_z = ret->point.z;
+    6854             :   }
+    6855             : 
+    6856             :   // | ------------ overwrite from estimation manager ----------- |
+    6857             : 
+    6858       13910 :   double estimation_manager_max_z = std::numeric_limits<float>::max();
+    6859             : 
+    6860             :   {
+    6861             :     // if possible, override it with max z from the estimation manager
+    6862       13910 :     if (sh_max_z_.hasMsg()) {
+    6863             : 
+    6864       27798 :       auto msg = sh_max_z_.getMsg();
+    6865             : 
+    6866             :       // transform it into the safety area frame
+    6867       27798 :       geometry_msgs::PointStamped point;
+    6868       13899 :       point.header  = msg->header;
+    6869       13899 :       point.point.x = 0;
+    6870       13899 :       point.point.y = 0;
+    6871       13899 :       point.point.z = msg->value;
+    6872             : 
+    6873       13899 :       auto ret = transformer_->transformSingle(point, frame_id);
+    6874             : 
+    6875       13899 :       if (!ret) {
+    6876           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform estimation manager's max_z to the current control frame");
+    6877             :       }
+    6878             : 
+    6879       13899 :       estimation_manager_max_z = ret->point.z;
+    6880             :     }
+    6881             :   }
+    6882             : 
+    6883       13910 :   if (estimation_manager_max_z < safety_area_max_z) {
+    6884       10471 :     return estimation_manager_max_z;
+    6885             :   } else {
+    6886        3439 :     return safety_area_max_z;
+    6887             :   }
+    6888             : }
+    6889             : 
+    6890             : //}
+    6891             : 
+    6892             : /* //{ getMinZ() */
+    6893             : 
+    6894       13913 : double ControlManager::getMinZ(const std::string& frame_id) {
+    6895             : 
+    6896       13913 :   if (!use_safety_area_) {
+    6897           0 :     return std::numeric_limits<double>::lowest();
+    6898             :   }
+    6899             : 
+    6900       27826 :   geometry_msgs::PointStamped point;
+    6901             : 
+    6902       13913 :   point.header.frame_id = _safety_area_vertical_frame_;
+    6903       13913 :   point.point.x         = 0;
+    6904       13913 :   point.point.y         = 0;
+    6905       13913 :   point.point.z         = _safety_area_min_z_;
+    6906             : 
+    6907       27826 :   auto ret = transformer_->transformSingle(point, frame_id);
+    6908             : 
+    6909       13913 :   if (!ret) {
+    6910           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform safety area's min_z to '%s'", frame_id.c_str());
+    6911           0 :     return std::numeric_limits<double>::lowest();
+    6912             :   }
+    6913             : 
+    6914       13913 :   return ret->point.z;
+    6915             : }
+    6916             : 
+    6917             : //}
+    6918             : 
+    6919             : // | --------------------- obstacle bumper -------------------- |
+    6920             : 
+    6921             : /* bumperPushFromObstacle() //{ */
+    6922             : 
+    6923         276 : void ControlManager::bumperPushFromObstacle(void) {
+    6924             : 
+    6925             :   // | --------------- fabricate the min distances -------------- |
+    6926             : 
+    6927         276 :   double min_distance_horizontal = _bumper_horizontal_distance_;
+    6928         276 :   double min_distance_vertical   = _bumper_vertical_distance_;
+    6929             : 
+    6930         276 :   if (_bumper_horizontal_derive_from_dynamics_ || _bumper_vertical_derive_from_dynamics_) {
+    6931             : 
+    6932         276 :     auto constraints = mrs_lib::get_mutexed(mutex_constraints_, current_constraints_);
+    6933             : 
+    6934         276 :     if (_bumper_horizontal_derive_from_dynamics_) {
+    6935             : 
+    6936         276 :       const double horizontal_t_stop    = constraints.constraints.horizontal_speed / constraints.constraints.horizontal_acceleration;
+    6937         276 :       const double horizontal_stop_dist = (horizontal_t_stop * constraints.constraints.horizontal_speed) / 2.0;
+    6938             : 
+    6939         276 :       min_distance_horizontal += 1.5 * horizontal_stop_dist;
+    6940             :     }
+    6941             : 
+    6942         276 :     if (_bumper_vertical_derive_from_dynamics_) {
+    6943             : 
+    6944             : 
+    6945             :       // larger from the two accelerations
+    6946         552 :       const double vert_acc = constraints.constraints.vertical_ascending_acceleration > constraints.constraints.vertical_descending_acceleration
+    6947         276 :                                   ? constraints.constraints.vertical_ascending_acceleration
+    6948             :                                   : constraints.constraints.vertical_descending_acceleration;
+    6949             : 
+    6950             :       // larger from the two speeds
+    6951         552 :       const double vert_speed = constraints.constraints.vertical_ascending_speed > constraints.constraints.vertical_descending_speed
+    6952         276 :                                     ? constraints.constraints.vertical_ascending_speed
+    6953             :                                     : constraints.constraints.vertical_descending_speed;
+    6954             : 
+    6955         276 :       const double vertical_t_stop    = vert_speed / vert_acc;
+    6956         276 :       const double vertical_stop_dist = (vertical_t_stop * vert_speed) / 2.0;
+    6957             : 
+    6958         276 :       min_distance_vertical += 1.5 * vertical_stop_dist;
+    6959             :     }
+    6960             :   }
+    6961             : 
+    6962             :   // | ----------------------------  ---------------------------- |
+    6963             : 
+    6964             :   // copy member variables
+    6965         276 :   mrs_msgs::ObstacleSectorsConstPtr bumper_data = sh_bumper_.getMsg();
+    6966         276 :   auto                              uav_state   = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    6967             : 
+    6968         276 :   double sector_size = TAU / double(bumper_data->n_horizontal_sectors);
+    6969             : 
+    6970         276 :   double direction          = 0;
+    6971         276 :   double repulsion_distance = std::numeric_limits<double>::max();
+    6972             : 
+    6973         276 :   bool horizontal_collision_detected = false;
+    6974         276 :   bool vertical_collision_detected   = false;
+    6975             : 
+    6976         276 :   double min_horizontal_sector_distance = std::numeric_limits<double>::max();
+    6977         276 :   size_t min_sector_id                  = 0;
+    6978             : 
+    6979        2484 :   for (unsigned long i = 0; i < bumper_data->n_horizontal_sectors; i++) {
+    6980             : 
+    6981        2208 :     if (bumper_data->sectors.at(i) < 0) {
+    6982           0 :       continue;
+    6983             :     }
+    6984             : 
+    6985        2208 :     if (bumper_data->sectors.at(i) < min_horizontal_sector_distance) {
+    6986         276 :       min_horizontal_sector_distance = bumper_data->sectors.at(i);
+    6987         276 :       min_sector_id                  = i;
+    6988             :     }
+    6989             :   }
+    6990             : 
+    6991             :   // if the sector is under the threshold distance
+    6992         276 :   if (min_horizontal_sector_distance < min_distance_horizontal) {
+    6993             : 
+    6994             :     // get the desired direction of motion
+    6995         104 :     double oposite_direction  = double(min_sector_id) * sector_size + M_PI;
+    6996         104 :     int    oposite_sector_idx = bumperGetSectorId(cos(oposite_direction), sin(oposite_direction), 0);
+    6997             : 
+    6998             :     // get the id of the oposite sector
+    6999         104 :     direction = oposite_direction;
+    7000             : 
+    7001         104 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: Bumper: found potential collision (sector %lu vs. %d), obstacle distance: %.2f, repulsing", min_sector_id,
+    7002             :                       oposite_sector_idx, bumper_data->sectors.at(min_sector_id));
+    7003             : 
+    7004         104 :     repulsion_distance = min_distance_horizontal + _bumper_horizontal_overshoot_ - bumper_data->sectors.at(min_sector_id);
+    7005             : 
+    7006         104 :     horizontal_collision_detected = true;
+    7007             :   }
+    7008             : 
+    7009         276 :   double vertical_repulsion_distance = 0;
+    7010             : 
+    7011             :   // check for vertical collision down
+    7012         276 :   if (bumper_data->sectors.at(bumper_data->n_horizontal_sectors) > 0 && bumper_data->sectors.at(bumper_data->n_horizontal_sectors) <= min_distance_vertical) {
+    7013             : 
+    7014           0 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: potential collision below");
+    7015           0 :     vertical_collision_detected = true;
+    7016           0 :     vertical_repulsion_distance = min_distance_vertical - bumper_data->sectors.at(bumper_data->n_horizontal_sectors);
+    7017             :   }
+    7018             : 
+    7019             :   // check for vertical collision up
+    7020         552 :   if (bumper_data->sectors.at(bumper_data->n_horizontal_sectors + 1) > 0 &&
+    7021         276 :       bumper_data->sectors.at(bumper_data->n_horizontal_sectors + 1) <= min_distance_vertical) {
+    7022             : 
+    7023           0 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: potential collision above");
+    7024           0 :     vertical_collision_detected = true;
+    7025           0 :     vertical_repulsion_distance = -(min_distance_vertical - bumper_data->sectors.at(bumper_data->n_horizontal_sectors + 1));
+    7026             :   }
+    7027             : 
+    7028             :   // if potential collision was detected and we should start the repulsing_
+    7029         276 :   if (horizontal_collision_detected || vertical_collision_detected) {
+    7030             : 
+    7031         104 :     if (!bumper_repulsing_) {
+    7032             : 
+    7033           1 :       if (_bumper_switch_tracker_) {
+    7034             : 
+    7035           1 :         auto        active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7036           2 :         std::string active_tracker_name = _tracker_names_.at(active_tracker_idx);
+    7037             : 
+    7038             :         // remember the previously active tracker
+    7039           1 :         bumper_previous_tracker_ = active_tracker_name;
+    7040             : 
+    7041           1 :         if (active_tracker_name != _bumper_tracker_name_) {
+    7042             : 
+    7043           0 :           switchTracker(_bumper_tracker_name_);
+    7044             :         }
+    7045             :       }
+    7046             : 
+    7047           1 :       if (_bumper_switch_controller_) {
+    7048             : 
+    7049           1 :         auto        active_controller_idx  = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    7050           2 :         std::string active_controller_name = _controller_names_.at(active_controller_idx);
+    7051             : 
+    7052             :         // remember the previously active controller
+    7053           1 :         bumper_previous_controller_ = active_controller_name;
+    7054             : 
+    7055           1 :         if (active_controller_name != _bumper_controller_name_) {
+    7056             : 
+    7057           1 :           switchController(_bumper_controller_name_);
+    7058             :         }
+    7059             :       }
+    7060             :     }
+    7061             : 
+    7062         104 :     bumper_repulsing_ = true;
+    7063             : 
+    7064         104 :     callbacks_enabled_ = false;
+    7065             : 
+    7066           0 :     mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;
+    7067             : 
+    7068         104 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    7069             : 
+    7070             :     // create the reference in the fcu_untilted frame
+    7071         104 :     mrs_msgs::ReferenceStamped reference_fcu_untilted;
+    7072             : 
+    7073         104 :     reference_fcu_untilted.header.frame_id = "fcu_untilted";
+    7074             : 
+    7075         104 :     if (horizontal_collision_detected) {
+    7076         104 :       reference_fcu_untilted.reference.position.x = cos(direction) * repulsion_distance;
+    7077         104 :       reference_fcu_untilted.reference.position.y = sin(direction) * repulsion_distance;
+    7078             :     } else {
+    7079           0 :       reference_fcu_untilted.reference.position.x = 0;
+    7080           0 :       reference_fcu_untilted.reference.position.y = 0;
+    7081             :     }
+    7082             : 
+    7083         104 :     reference_fcu_untilted.reference.heading = 0;
+    7084             : 
+    7085         104 :     if (vertical_collision_detected) {
+    7086           0 :       reference_fcu_untilted.reference.position.z = vertical_repulsion_distance;
+    7087             :     } else {
+    7088         104 :       reference_fcu_untilted.reference.position.z = 0;
+    7089             :     }
+    7090             : 
+    7091             :     {
+    7092         104 :       std::scoped_lock lock(mutex_tracker_list_);
+    7093             : 
+    7094             :       // transform the reference into the currently used frame
+    7095             :       // this is under the mutex_tracker_list since we don't won't the odometry switch to happen
+    7096             :       // to the tracker before we actually call the goto service
+    7097             : 
+    7098         104 :       auto ret = transformer_->transformSingle(reference_fcu_untilted, uav_state.header.frame_id);
+    7099             : 
+    7100         104 :       if (!ret) {
+    7101           0 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: Bumper: bumper reference could not be transformed");
+    7102           0 :         return;
+    7103             :       }
+    7104             : 
+    7105         104 :       reference_fcu_untilted = ret.value();
+    7106             : 
+    7107             :       // copy the reference into the service type message
+    7108         104 :       mrs_msgs::ReferenceSrvRequest req_goto_out;
+    7109         104 :       req_goto_out.reference = reference_fcu_untilted.reference;
+    7110             : 
+    7111             :       // disable callbacks of all trackers
+    7112         104 :       req_enable_callbacks.data = false;
+    7113         728 :       for (size_t i = 0; i < tracker_list_.size(); i++) {
+    7114         624 :         tracker_list_.at(i)->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    7115             :       }
+    7116             : 
+    7117             :       // enable the callbacks for the active tracker
+    7118         104 :       req_enable_callbacks.data = true;
+    7119         104 :       tracker_list_.at(active_tracker_idx_)
+    7120         104 :           ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    7121             : 
+    7122             :       // call the goto
+    7123         104 :       tracker_response = tracker_list_.at(active_tracker_idx_)
+    7124         104 :                              ->setReference(mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(req_goto_out)));
+    7125             : 
+    7126             :       // disable the callbacks back again
+    7127         104 :       req_enable_callbacks.data = false;
+    7128         104 :       tracker_list_.at(active_tracker_idx_)
+    7129         104 :           ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    7130             :     }
+    7131             :   }
+    7132             : 
+    7133             :   // if repulsing_ and the distance is safe once again
+    7134         276 :   if (bumper_repulsing_ && !horizontal_collision_detected && !vertical_collision_detected) {
+    7135             : 
+    7136           1 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: no more collision, stopping repulsion");
+    7137             : 
+    7138           1 :     if (_bumper_switch_tracker_) {
+    7139             : 
+    7140           1 :       auto        active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7141           2 :       std::string active_tracker_name = _tracker_names_.at(active_tracker_idx);
+    7142             : 
+    7143           1 :       if (active_tracker_name != bumper_previous_tracker_) {
+    7144             : 
+    7145           0 :         switchTracker(bumper_previous_tracker_);
+    7146             :       }
+    7147             :     }
+    7148             : 
+    7149           1 :     if (_bumper_switch_controller_) {
+    7150             : 
+    7151           1 :       auto        active_controller_idx  = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    7152           2 :       std::string active_controller_name = _controller_names_.at(active_controller_idx);
+    7153             : 
+    7154           1 :       if (active_controller_name != bumper_previous_controller_) {
+    7155             : 
+    7156           1 :         switchController(bumper_previous_controller_);
+    7157             :       }
+    7158             :     }
+    7159             : 
+    7160           1 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    7161             : 
+    7162             :     {
+    7163           2 :       std::scoped_lock lock(mutex_tracker_list_);
+    7164             : 
+    7165             :       // enable callbacks of all trackers
+    7166           1 :       req_enable_callbacks.data = true;
+    7167           7 :       for (size_t i = 0; i < tracker_list_.size(); i++) {
+    7168           6 :         tracker_list_.at(i)->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    7169             :       }
+    7170             :     }
+    7171             : 
+    7172           1 :     callbacks_enabled_ = true;
+    7173             : 
+    7174           1 :     bumper_repulsing_ = false;
+    7175             :   }
+    7176             : }
+    7177             : 
+    7178             : //}
+    7179             : 
+    7180             : /* bumperGetSectorId() //{ */
+    7181             : 
+    7182         104 : int ControlManager::bumperGetSectorId(const double& x, const double& y, [[maybe_unused]] const double& z) {
+    7183             : 
+    7184             :   // copy member variables
+    7185         104 :   mrs_msgs::ObstacleSectorsConstPtr bumper_data = sh_bumper_.getMsg();
+    7186             : 
+    7187             :   // heading of the point in drone frame
+    7188         104 :   double point_heading_horizontal = atan2(y, x);
+    7189             : 
+    7190         104 :   point_heading_horizontal += TAU;
+    7191             : 
+    7192             :   // if point_heading_horizontal is greater then 2*M_PI mod it
+    7193         104 :   if (fabs(point_heading_horizontal) >= TAU) {
+    7194         104 :     point_heading_horizontal = fmod(point_heading_horizontal, TAU);
+    7195             :   }
+    7196             : 
+    7197             :   // heading of the right edge of the first sector
+    7198         104 :   double sector_size = TAU / double(bumper_data->n_horizontal_sectors);
+    7199             : 
+    7200             :   // calculate the idx
+    7201         104 :   int idx = floor((point_heading_horizontal + (sector_size / 2.0)) / sector_size);
+    7202             : 
+    7203         104 :   if (idx > int(bumper_data->n_horizontal_sectors) - 1) {
+    7204           0 :     idx -= bumper_data->n_horizontal_sectors;
+    7205             :   }
+    7206             : 
+    7207         208 :   return idx;
+    7208             : }
+    7209             : 
+    7210             : //}
+    7211             : 
+    7212             : // | ------------------------- safety ------------------------- |
+    7213             : 
+    7214             : /* //{ changeLandingState() */
+    7215             : 
+    7216          12 : void ControlManager::changeLandingState(LandingStates_t new_state) {
+    7217             : 
+    7218             :   // copy member variables
+    7219          24 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7220             : 
+    7221             :   {
+    7222          12 :     std::scoped_lock lock(mutex_landing_state_machine_);
+    7223             : 
+    7224          12 :     previous_state_landing_ = current_state_landing_;
+    7225          12 :     current_state_landing_  = new_state;
+    7226             :   }
+    7227             : 
+    7228          12 :   switch (current_state_landing_) {
+    7229             : 
+    7230           4 :     case IDLE_STATE:
+    7231           4 :       break;
+    7232           8 :     case LANDING_STATE: {
+    7233             : 
+    7234           8 :       ROS_DEBUG("[ControlManager]: starting eland timer");
+    7235           8 :       timer_eland_.start();
+    7236           8 :       ROS_DEBUG("[ControlManager]: eland timer started");
+    7237           8 :       eland_triggered_ = true;
+    7238           8 :       bumper_enabled_  = false;
+    7239             : 
+    7240           8 :       landing_uav_mass_ = getMass();
+    7241             :     }
+    7242             : 
+    7243           8 :     break;
+    7244             :   }
+    7245             : 
+    7246          12 :   ROS_INFO("[ControlManager]: switching emergency landing state %s -> %s", state_names[previous_state_landing_], state_names[current_state_landing_]);
+    7247          12 : }
+    7248             : 
+    7249             : //}
+    7250             : 
+    7251             : /* hover() //{ */
+    7252             : 
+    7253           1 : std::tuple<bool, std::string> ControlManager::hover(void) {
+    7254             : 
+    7255           1 :   if (!is_initialized_) {
+    7256           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7257             :   }
+    7258             : 
+    7259           1 :   if (eland_triggered_) {
+    7260           0 :     return std::tuple(false, "cannot hover, eland already triggered");
+    7261             :   }
+    7262             : 
+    7263           1 :   if (failsafe_triggered_) {
+    7264           0 :     return std::tuple(false, "cannot hover, failsafe already triggered");
+    7265             :   }
+    7266             : 
+    7267             :   {
+    7268           2 :     std::scoped_lock lock(mutex_tracker_list_);
+    7269             : 
+    7270           1 :     std_srvs::TriggerResponse::ConstPtr response;
+    7271           1 :     std_srvs::TriggerRequest            request;
+    7272             : 
+    7273           1 :     response = tracker_list_.at(active_tracker_idx_)->hover(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7274             : 
+    7275           1 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7276             : 
+    7277           2 :       return std::tuple(response->success, response->message);
+    7278             : 
+    7279             :     } else {
+    7280             : 
+    7281           0 :       std::stringstream ss;
+    7282           0 :       ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'hover()' function!";
+    7283             : 
+    7284           0 :       return std::tuple(false, ss.str());
+    7285             :     }
+    7286             :   }
+    7287             : }
+    7288             : 
+    7289             : //}
+    7290             : 
+    7291             : /* //{ ehover() */
+    7292             : 
+    7293           4 : std::tuple<bool, std::string> ControlManager::ehover(void) {
+    7294             : 
+    7295           4 :   if (!is_initialized_) {
+    7296           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7297             :   }
+    7298             : 
+    7299           4 :   if (eland_triggered_) {
+    7300           0 :     return std::tuple(false, "cannot ehover, eland already triggered");
+    7301             :   }
+    7302             : 
+    7303           4 :   if (failsafe_triggered_) {
+    7304           0 :     return std::tuple(false, "cannot ehover, failsafe already triggered");
+    7305             :   }
+    7306             : 
+    7307             :   // copy the member variables
+    7308           8 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7309           4 :   auto active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7310             : 
+    7311           4 :   if (active_tracker_idx == _null_tracker_idx_) {
+    7312             : 
+    7313           0 :     std::stringstream ss;
+    7314           0 :     ss << "can not trigger ehover while not flying";
+    7315           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7316             : 
+    7317           0 :     return std::tuple(false, ss.str());
+    7318             :   }
+    7319             : 
+    7320           4 :   ungripSrv();
+    7321             : 
+    7322             :   {
+    7323             : 
+    7324           4 :     auto [success, message] = switchTracker(_ehover_tracker_name_);
+    7325             : 
+    7326             :     // check if the tracker was successfully switched
+    7327             :     // this is vital, that is the core of the hover
+    7328           4 :     if (!success) {
+    7329             : 
+    7330           0 :       std::stringstream ss;
+    7331           0 :       ss << "error during switching to ehover tracker: '" << message << "'";
+    7332           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7333             : 
+    7334           0 :       return std::tuple(success, ss.str());
+    7335             :     }
+    7336             :   }
+    7337             : 
+    7338             :   {
+    7339           8 :     auto [success, message] = switchController(_eland_controller_name_);
+    7340             : 
+    7341             :     // check if the controller was successfully switched
+    7342             :     // this is not vital, we can continue without that
+    7343           4 :     if (!success) {
+    7344             : 
+    7345           0 :       std::stringstream ss;
+    7346           0 :       ss << "error during switching to ehover controller: '" << message << "'";
+    7347           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7348             :     }
+    7349             :   }
+    7350             : 
+    7351           4 :   std::stringstream ss;
+    7352           4 :   ss << "ehover activated";
+    7353           4 :   ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7354             : 
+    7355           4 :   callbacks_enabled_ = false;
+    7356             : 
+    7357           4 :   return std::tuple(true, ss.str());
+    7358             : }
+    7359             : 
+    7360             : //}
+    7361             : 
+    7362             : /* eland() //{ */
+    7363             : 
+    7364           8 : std::tuple<bool, std::string> ControlManager::eland(void) {
+    7365             : 
+    7366           8 :   if (!is_initialized_) {
+    7367           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7368             :   }
+    7369             : 
+    7370           8 :   if (eland_triggered_) {
+    7371           0 :     return std::tuple(false, "cannot eland, eland already triggered");
+    7372             :   }
+    7373             : 
+    7374           8 :   if (failsafe_triggered_) {
+    7375           0 :     return std::tuple(false, "cannot eland, failsafe already triggered");
+    7376             :   }
+    7377             : 
+    7378             :   // copy member variables
+    7379          16 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7380           8 :   auto active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7381             : 
+    7382           8 :   if (active_tracker_idx == _null_tracker_idx_) {
+    7383             : 
+    7384           0 :     std::stringstream ss;
+    7385           0 :     ss << "can not trigger eland while not flying";
+    7386           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7387             : 
+    7388           0 :     return std::tuple(false, ss.str());
+    7389             :   }
+    7390             : 
+    7391           8 :   if (_rc_emergency_handoff_) {
+    7392             : 
+    7393           0 :     toggleOutput(false);
+    7394             : 
+    7395           0 :     return std::tuple(true, "RC emergency handoff is ON, disabling output");
+    7396             :   }
+    7397             : 
+    7398             :   {
+    7399           8 :     auto [success, message] = switchTracker(_ehover_tracker_name_);
+    7400             : 
+    7401             :     // check if the tracker was successfully switched
+    7402             :     // this is vital
+    7403           8 :     if (!success) {
+    7404             : 
+    7405           0 :       std::stringstream ss;
+    7406           0 :       ss << "error during switching to eland tracker: '" << message << "'";
+    7407           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7408             : 
+    7409           0 :       return std::tuple(success, ss.str());
+    7410             :     }
+    7411             :   }
+    7412             : 
+    7413             :   {
+    7414          16 :     auto [success, message] = switchController(_eland_controller_name_);
+    7415             : 
+    7416             :     // check if the controller was successfully switched
+    7417             :     // this is not vital, we can continue without it
+    7418           8 :     if (!success) {
+    7419             : 
+    7420           0 :       std::stringstream ss;
+    7421           0 :       ss << "error during switching to eland controller: '" << message << "'";
+    7422           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7423             :     }
+    7424             :   }
+    7425             : 
+    7426             :   // | ----------------- call the eland service ----------------- |
+    7427             : 
+    7428           8 :   std::stringstream ss;
+    7429             :   bool              success;
+    7430             : 
+    7431           8 :   if (elandSrv()) {
+    7432             : 
+    7433           8 :     changeLandingState(LANDING_STATE);
+    7434             : 
+    7435           8 :     odometryCallbacksSrv(false);
+    7436             : 
+    7437           8 :     ss << "eland activated";
+    7438           8 :     ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7439             : 
+    7440           8 :     success = true;
+    7441             : 
+    7442           8 :     callbacks_enabled_ = false;
+    7443             : 
+    7444             :   } else {
+    7445             : 
+    7446           0 :     ss << "error during activation of eland";
+    7447           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7448             : 
+    7449           0 :     success = false;
+    7450             :   }
+    7451             : 
+    7452          16 :   return std::tuple(success, ss.str());
+    7453             : }
+    7454             : 
+    7455             : //}
+    7456             : 
+    7457             : /* failsafe() //{ */
+    7458             : 
+    7459           7 : std::tuple<bool, std::string> ControlManager::failsafe(void) {
+    7460             : 
+    7461             :   // copy member variables
+    7462          14 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7463           7 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    7464           7 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7465             : 
+    7466           7 :   if (!is_initialized_) {
+    7467           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7468             :   }
+    7469             : 
+    7470           7 :   if (failsafe_triggered_) {
+    7471           0 :     return std::tuple(false, "cannot, failsafe already triggered");
+    7472             :   }
+    7473             : 
+    7474           7 :   if (active_tracker_idx == _null_tracker_idx_) {
+    7475             : 
+    7476           0 :     std::stringstream ss;
+    7477           0 :     ss << "can not trigger failsafe while not flying";
+    7478           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7479           0 :     return std::tuple(false, ss.str());
+    7480             :   }
+    7481             : 
+    7482           7 :   if (_rc_emergency_handoff_) {
+    7483             : 
+    7484           0 :     toggleOutput(false);
+    7485             : 
+    7486           0 :     return std::tuple(true, "RC emergency handoff is ON, disabling output");
+    7487             :   }
+    7488             : 
+    7489           7 :   if (getLowestOuput(_hw_api_inputs_) == POSITION) {
+    7490           0 :     return eland();
+    7491             :   }
+    7492             : 
+    7493           7 :   if (_parachute_enabled_) {
+    7494             : 
+    7495           0 :     auto [success, message] = deployParachute();
+    7496             : 
+    7497           0 :     if (success) {
+    7498             : 
+    7499           0 :       std::stringstream ss;
+    7500           0 :       ss << "failsafe activated (parachute): '" << message << "'";
+    7501           0 :       ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    7502             : 
+    7503           0 :       return std::tuple(true, ss.str());
+    7504             : 
+    7505             :     } else {
+    7506             : 
+    7507           0 :       std::stringstream ss;
+    7508           0 :       ss << "could not deploy parachute: '" << message << "', continuing with normal failsafe";
+    7509           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7510             :     }
+    7511             :   }
+    7512             : 
+    7513           7 :   if (_failsafe_controller_idx_ != active_controller_idx) {
+    7514             : 
+    7515             :     try {
+    7516             : 
+    7517          14 :       std::scoped_lock lock(mutex_controller_list_);
+    7518             : 
+    7519           7 :       ROS_INFO("[ControlManager]: activating the controller '%s'", _failsafe_controller_name_.c_str());
+    7520           7 :       controller_list_.at(_failsafe_controller_idx_)->activate(last_control_output);
+    7521             : 
+    7522             :       {
+    7523          14 :         std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    7524             : 
+    7525             :         // update the time (used in failsafe)
+    7526           7 :         controller_tracker_switch_time_ = ros::Time::now();
+    7527             :       }
+    7528             : 
+    7529           7 :       failsafe_triggered_ = true;
+    7530           7 :       ROS_DEBUG("[ControlManager]: stopping eland timer");
+    7531           7 :       timer_eland_.stop();
+    7532           7 :       ROS_DEBUG("[ControlManager]: eland timer stopped");
+    7533             : 
+    7534           7 :       landing_uav_mass_ = getMass();
+    7535             : 
+    7536           7 :       eland_triggered_ = false;
+    7537           7 :       ROS_DEBUG("[ControlManager]: starting failsafe timer");
+    7538           7 :       timer_failsafe_.start();
+    7539           7 :       ROS_DEBUG("[ControlManager]: failsafe timer started");
+    7540             : 
+    7541           7 :       bumper_enabled_ = false;
+    7542             : 
+    7543           7 :       odometryCallbacksSrv(false);
+    7544             : 
+    7545           7 :       callbacks_enabled_ = false;
+    7546             : 
+    7547           7 :       ROS_INFO_THROTTLE(1.0, "[ControlManager]: the controller '%s' was activated", _failsafe_controller_name_.c_str());
+    7548             : 
+    7549             :       // super important, switch the active controller idx
+    7550             :       try {
+    7551           7 :         controller_list_.at(active_controller_idx_)->deactivate();
+    7552           7 :         active_controller_idx_ = _failsafe_controller_idx_;
+    7553             :       }
+    7554           0 :       catch (std::runtime_error& exrun) {
+    7555           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not deactivate the controller '%s'", _controller_names_.at(active_controller_idx_).c_str());
+    7556             :       }
+    7557             :     }
+    7558           0 :     catch (std::runtime_error& exrun) {
+    7559           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: error during activation of the controller '%s'", _failsafe_controller_name_.c_str());
+    7560           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+    7561             :     }
+    7562             :   }
+    7563             : 
+    7564          14 :   return std::tuple(true, "failsafe activated");
+    7565             : }
+    7566             : 
+    7567             : //}
+    7568             : 
+    7569             : /* escalatingFailsafe() //{ */
+    7570             : 
+    7571         147 : std::tuple<bool, std::string> ControlManager::escalatingFailsafe(void) {
+    7572             : 
+    7573         294 :   std::stringstream ss;
+    7574             : 
+    7575         147 :   if ((ros::Time::now() - escalating_failsafe_time_).toSec() < _escalating_failsafe_timeout_) {
+    7576             : 
+    7577         139 :     ss << "too soon for escalating failsafe";
+    7578         139 :     ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7579             : 
+    7580         139 :     return std::tuple(false, ss.str());
+    7581             :   }
+    7582             : 
+    7583           8 :   if (!output_enabled_) {
+    7584             : 
+    7585           0 :     ss << "not escalating failsafe, output is disabled";
+    7586           0 :     ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7587             : 
+    7588           0 :     return std::tuple(false, ss.str());
+    7589             :   }
+    7590             : 
+    7591           8 :   ROS_WARN("[ControlManager]: escalating failsafe triggered");
+    7592             : 
+    7593           8 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7594           8 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    7595             : 
+    7596          16 :   std::string active_tracker_name    = _tracker_names_.at(active_tracker_idx);
+    7597          16 :   std::string active_controller_name = _controller_names_.at(active_controller_idx);
+    7598             : 
+    7599           8 :   EscalatingFailsafeStates_t next_state = getNextEscFailsafeState();
+    7600             : 
+    7601           8 :   escalating_failsafe_time_ = ros::Time::now();
+    7602             : 
+    7603           8 :   switch (next_state) {
+    7604             : 
+    7605           0 :     case ESC_NONE_STATE: {
+    7606             : 
+    7607           0 :       ss << "escalating failsafe has run to impossible situation";
+    7608           0 :       ROS_ERROR_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7609             : 
+    7610           0 :       return std::tuple(false, "escalating failsafe has run to impossible situation");
+    7611             : 
+    7612             :       break;
+    7613             :     }
+    7614             : 
+    7615           2 :     case ESC_EHOVER_STATE: {
+    7616             : 
+    7617           2 :       ss << "escalating failsafe escalates to ehover";
+    7618           2 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7619             : 
+    7620           4 :       auto [success, message] = ehover();
+    7621             : 
+    7622           2 :       if (success) {
+    7623           2 :         state_escalating_failsafe_ = ESC_EHOVER_STATE;
+    7624             :       }
+    7625             : 
+    7626           2 :       return {success, message};
+    7627             : 
+    7628             :       break;
+    7629             :     }
+    7630             : 
+    7631           2 :     case ESC_ELAND_STATE: {
+    7632             : 
+    7633           2 :       ss << "escalating failsafe escalates to eland";
+    7634           2 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7635             : 
+    7636           4 :       auto [success, message] = eland();
+    7637             : 
+    7638           2 :       if (success) {
+    7639           2 :         state_escalating_failsafe_ = ESC_ELAND_STATE;
+    7640             :       }
+    7641             : 
+    7642           2 :       return {success, message};
+    7643             : 
+    7644             :       break;
+    7645             :     }
+    7646             : 
+    7647           2 :     case ESC_FAILSAFE_STATE: {
+    7648             : 
+    7649           2 :       escalating_failsafe_time_ = ros::Time::now();
+    7650             : 
+    7651           2 :       ss << "escalating failsafe escalates to failsafe";
+    7652           2 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7653             : 
+    7654           4 :       auto [success, message] = failsafe();
+    7655             : 
+    7656           2 :       if (success) {
+    7657           2 :         state_escalating_failsafe_ = ESC_FINISHED_STATE;
+    7658             :       }
+    7659             : 
+    7660           2 :       return {success, message};
+    7661             : 
+    7662             :       break;
+    7663             :     }
+    7664             : 
+    7665           2 :     case ESC_FINISHED_STATE: {
+    7666             : 
+    7667           2 :       escalating_failsafe_time_ = ros::Time::now();
+    7668             : 
+    7669           2 :       ss << "escalating failsafe has nothing more to do";
+    7670           2 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7671             : 
+    7672           4 :       return std::tuple(false, "escalating failsafe has nothing more to do");
+    7673             : 
+    7674             :       break;
+    7675             :     }
+    7676             : 
+    7677           0 :     default: {
+    7678             : 
+    7679           0 :       break;
+    7680             :     }
+    7681             :   }
+    7682             : 
+    7683           0 :   ROS_ERROR("[ControlManager]: escalatingFailsafe() reached the final return, this should not happen!");
+    7684             : 
+    7685           0 :   return std::tuple(false, "escalating failsafe exception");
+    7686             : }
+    7687             : 
+    7688             : //}
+    7689             : 
+    7690             : /* getNextEscFailsafeState() //{ */
+    7691             : 
+    7692           8 : EscalatingFailsafeStates_t ControlManager::getNextEscFailsafeState(void) {
+    7693             : 
+    7694           8 :   EscalatingFailsafeStates_t current_state = state_escalating_failsafe_;
+    7695             : 
+    7696           8 :   switch (current_state) {
+    7697             : 
+    7698           2 :     case ESC_FINISHED_STATE: {
+    7699             : 
+    7700           2 :       return ESC_FINISHED_STATE;
+    7701             : 
+    7702             :       break;
+    7703             :     }
+    7704             : 
+    7705           2 :     case ESC_NONE_STATE: {
+    7706             : 
+    7707           2 :       if (_escalating_failsafe_ehover_) {
+    7708           2 :         return ESC_EHOVER_STATE;
+    7709           0 :       } else if (_escalating_failsafe_eland_) {
+    7710           0 :         return ESC_ELAND_STATE;
+    7711           0 :       } else if (_escalating_failsafe_failsafe_) {
+    7712           0 :         return ESC_FAILSAFE_STATE;
+    7713             :       } else {
+    7714           0 :         return ESC_FINISHED_STATE;
+    7715             :       }
+    7716             : 
+    7717             :       break;
+    7718             :     }
+    7719             : 
+    7720           2 :     case ESC_EHOVER_STATE: {
+    7721             : 
+    7722           2 :       if (_escalating_failsafe_eland_) {
+    7723           2 :         return ESC_ELAND_STATE;
+    7724           0 :       } else if (_escalating_failsafe_failsafe_) {
+    7725           0 :         return ESC_FAILSAFE_STATE;
+    7726             :       } else {
+    7727           0 :         return ESC_FINISHED_STATE;
+    7728             :       }
+    7729             : 
+    7730             :       break;
+    7731             :     }
+    7732             : 
+    7733           2 :     case ESC_ELAND_STATE: {
+    7734             : 
+    7735           2 :       if (_escalating_failsafe_failsafe_) {
+    7736           2 :         return ESC_FAILSAFE_STATE;
+    7737             :       } else {
+    7738           0 :         return ESC_FINISHED_STATE;
+    7739             :       }
+    7740             : 
+    7741             :       break;
+    7742             :     }
+    7743             : 
+    7744           0 :     case ESC_FAILSAFE_STATE: {
+    7745             : 
+    7746           0 :       return ESC_FINISHED_STATE;
+    7747             : 
+    7748             :       break;
+    7749             :     }
+    7750             :   }
+    7751             : 
+    7752           0 :   ROS_ERROR("[ControlManager]: getNextEscFailsafeState() reached the final return, this should not happen!");
+    7753             : 
+    7754           0 :   return ESC_NONE_STATE;
+    7755             : }
+    7756             : 
+    7757             : //}
+    7758             : 
+    7759             : // | ------------------- trajectory tracking ------------------ |
+    7760             : 
+    7761             : /* startTrajectoryTracking() //{ */
+    7762             : 
+    7763           2 : std::tuple<bool, std::string> ControlManager::startTrajectoryTracking(void) {
+    7764             : 
+    7765           2 :   if (!is_initialized_) {
+    7766           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7767             :   }
+    7768             : 
+    7769             :   {
+    7770           4 :     std::scoped_lock lock(mutex_tracker_list_);
+    7771             : 
+    7772           2 :     std_srvs::TriggerResponse::ConstPtr response;
+    7773           2 :     std_srvs::TriggerRequest            request;
+    7774             : 
+    7775             :     response =
+    7776           2 :         tracker_list_.at(active_tracker_idx_)->startTrajectoryTracking(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7777             : 
+    7778           2 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7779             : 
+    7780           4 :       return std::tuple(response->success, response->message);
+    7781             : 
+    7782             :     } else {
+    7783             : 
+    7784           0 :       std::stringstream ss;
+    7785           0 :       ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'startTrajectoryTracking()' function!";
+    7786             : 
+    7787           0 :       return std::tuple(false, ss.str());
+    7788             :     }
+    7789             :   }
+    7790             : }
+    7791             : 
+    7792             : //}
+    7793             : 
+    7794             : /* stopTrajectoryTracking() //{ */
+    7795             : 
+    7796           1 : std::tuple<bool, std::string> ControlManager::stopTrajectoryTracking(void) {
+    7797             : 
+    7798           1 :   if (!is_initialized_) {
+    7799           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7800             :   }
+    7801             : 
+    7802             :   {
+    7803           2 :     std::scoped_lock lock(mutex_tracker_list_);
+    7804             : 
+    7805           1 :     std_srvs::TriggerResponse::ConstPtr response;
+    7806           1 :     std_srvs::TriggerRequest            request;
+    7807             : 
+    7808             :     response =
+    7809           1 :         tracker_list_.at(active_tracker_idx_)->stopTrajectoryTracking(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7810             : 
+    7811           1 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7812             : 
+    7813           2 :       return std::tuple(response->success, response->message);
+    7814             : 
+    7815             :     } else {
+    7816             : 
+    7817           0 :       std::stringstream ss;
+    7818           0 :       ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'stopTrajectoryTracking()' function!";
+    7819             : 
+    7820           0 :       return std::tuple(false, ss.str());
+    7821             :     }
+    7822             :   }
+    7823             : }
+    7824             : 
+    7825             : //}
+    7826             : 
+    7827             : /* resumeTrajectoryTracking() //{ */
+    7828             : 
+    7829           1 : std::tuple<bool, std::string> ControlManager::resumeTrajectoryTracking(void) {
+    7830             : 
+    7831           1 :   if (!is_initialized_) {
+    7832           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7833             :   }
+    7834             : 
+    7835             :   {
+    7836           2 :     std::scoped_lock lock(mutex_tracker_list_);
+    7837             : 
+    7838           1 :     std_srvs::TriggerResponse::ConstPtr response;
+    7839           1 :     std_srvs::TriggerRequest            request;
+    7840             : 
+    7841           1 :     response = tracker_list_.at(active_tracker_idx_)
+    7842           1 :                    ->resumeTrajectoryTracking(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7843             : 
+    7844           1 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7845             : 
+    7846           2 :       return std::tuple(response->success, response->message);
+    7847             : 
+    7848             :     } else {
+    7849             : 
+    7850           0 :       std::stringstream ss;
+    7851           0 :       ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'resumeTrajectoryTracking()' function!";
+    7852             : 
+    7853           0 :       return std::tuple(false, ss.str());
+    7854             :     }
+    7855             :   }
+    7856             : }
+    7857             : 
+    7858             : //}
+    7859             : 
+    7860             : /* gotoTrajectoryStart() //{ */
+    7861             : 
+    7862           2 : std::tuple<bool, std::string> ControlManager::gotoTrajectoryStart(void) {
+    7863             : 
+    7864           2 :   if (!is_initialized_) {
+    7865           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7866             :   }
+    7867             : 
+    7868             :   {
+    7869           4 :     std::scoped_lock lock(mutex_tracker_list_);
+    7870             : 
+    7871           2 :     std_srvs::TriggerResponse::ConstPtr response;
+    7872           2 :     std_srvs::TriggerRequest            request;
+    7873             : 
+    7874             :     response =
+    7875           2 :         tracker_list_.at(active_tracker_idx_)->gotoTrajectoryStart(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7876             : 
+    7877           2 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7878             : 
+    7879           4 :       return std::tuple(response->success, response->message);
+    7880             : 
+    7881             :     } else {
+    7882             : 
+    7883           0 :       std::stringstream ss;
+    7884           0 :       ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'gotoTrajectoryStart()' function!";
+    7885             : 
+    7886           0 :       return std::tuple(false, ss.str());
+    7887             :     }
+    7888             :   }
+    7889             : }
+    7890             : 
+    7891             : //}
+    7892             : 
+    7893             : // | ----------------- service client wrappers ---------------- |
+    7894             : 
+    7895             : /* arming() //{ */
+    7896             : 
+    7897          18 : std::tuple<bool, std::string> ControlManager::arming(const bool input) {
+    7898             : 
+    7899          36 :   std::stringstream ss;
+    7900             : 
+    7901          18 :   if (input) {
+    7902             : 
+    7903           0 :     ss << "not allowed to arm using the ControlManager, maybe later when we don't do bugs";
+    7904           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7905           0 :     return std::tuple(false, ss.str());
+    7906             :   }
+    7907             : 
+    7908          18 :   if (!input && !isOffboard()) {
+    7909             : 
+    7910           0 :     ss << "can not disarm, not in OFFBOARD mode";
+    7911           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7912           0 :     return std::tuple(false, ss.str());
+    7913             :   }
+    7914             : 
+    7915          18 :   if (!input && _rc_emergency_handoff_) {
+    7916             : 
+    7917           0 :     toggleOutput(false);
+    7918             : 
+    7919           0 :     return std::tuple(true, "RC emergency handoff is ON, disabling output");
+    7920             :   }
+    7921             : 
+    7922          18 :   std_srvs::SetBool srv_out;
+    7923             : 
+    7924          18 :   srv_out.request.data = input ? 1 : 0;  // arm or disarm?
+    7925             : 
+    7926          18 :   ROS_INFO("[ControlManager]: calling for %s", input ? "arming" : "disarming");
+    7927             : 
+    7928          18 :   if (sch_arming_.call(srv_out)) {
+    7929             : 
+    7930          18 :     if (srv_out.response.success) {
+    7931             : 
+    7932          18 :       ss << "service call for " << (input ? "arming" : "disarming") << " was successful";
+    7933          18 :       ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7934             : 
+    7935          18 :       if (!input) {
+    7936             : 
+    7937          18 :         toggleOutput(false);
+    7938             : 
+    7939          18 :         ROS_DEBUG("[ControlManager]: stopping failsafe timer");
+    7940          18 :         timer_failsafe_.stop();
+    7941          18 :         ROS_DEBUG("[ControlManager]: failsafe timer stopped");
+    7942             : 
+    7943          18 :         ROS_DEBUG("[ControlManager]: stopping the eland timer");
+    7944          18 :         timer_eland_.stop();
+    7945          18 :         ROS_DEBUG("[ControlManager]: eland timer stopped");
+    7946             :       }
+    7947             : 
+    7948             :     } else {
+    7949           0 :       ss << "service call for " << (input ? "arming" : "disarming") << " failed";
+    7950           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7951             :     }
+    7952             : 
+    7953             :   } else {
+    7954           0 :     ss << "calling for " << (input ? "arming" : "disarming") << " resulted in failure: '" << srv_out.response.message << "'";
+    7955           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7956             :   }
+    7957             : 
+    7958          36 :   return std::tuple(srv_out.response.success, ss.str());
+    7959             : }
+    7960             : 
+    7961             : //}
+    7962             : 
+    7963             : /* odometryCallbacksSrv() //{ */
+    7964             : 
+    7965          15 : void ControlManager::odometryCallbacksSrv(const bool input) {
+    7966             : 
+    7967          15 :   ROS_INFO("[ControlManager]: switching odometry callbacks to %s", input ? "ON" : "OFF");
+    7968             : 
+    7969          30 :   std_srvs::SetBool srv;
+    7970             : 
+    7971          15 :   srv.request.data = input;
+    7972             : 
+    7973          15 :   bool res = sch_set_odometry_callbacks_.call(srv);
+    7974             : 
+    7975          15 :   if (res) {
+    7976             : 
+    7977          15 :     if (!srv.response.success) {
+    7978           0 :       ROS_WARN("[ControlManager]: service call for toggle odometry callbacks returned: '%s'", srv.response.message.c_str());
+    7979             :     }
+    7980             : 
+    7981             :   } else {
+    7982           0 :     ROS_ERROR("[ControlManager]: service call for toggle odometry callbacks failed!");
+    7983             :   }
+    7984          15 : }
+    7985             : 
+    7986             : //}
+    7987             : 
+    7988             : /* elandSrv() //{ */
+    7989             : 
+    7990           8 : bool ControlManager::elandSrv(void) {
+    7991             : 
+    7992           8 :   ROS_INFO("[ControlManager]: calling for eland");
+    7993             : 
+    7994          16 :   std_srvs::Trigger srv;
+    7995             : 
+    7996           8 :   bool res = sch_eland_.call(srv);
+    7997             : 
+    7998           8 :   if (res) {
+    7999             : 
+    8000           8 :     if (!srv.response.success) {
+    8001           0 :       ROS_WARN("[ControlManager]: service call for eland returned: '%s'", srv.response.message.c_str());
+    8002             :     }
+    8003             : 
+    8004           8 :     return srv.response.success;
+    8005             : 
+    8006             :   } else {
+    8007             : 
+    8008           0 :     ROS_ERROR("[ControlManager]: service call for eland failed!");
+    8009             : 
+    8010           0 :     return false;
+    8011             :   }
+    8012             : }
+    8013             : 
+    8014             : //}
+    8015             : 
+    8016             : /* parachuteSrv() //{ */
+    8017             : 
+    8018           0 : bool ControlManager::parachuteSrv(void) {
+    8019             : 
+    8020           0 :   ROS_INFO("[ControlManager]: calling for parachute deployment");
+    8021             : 
+    8022           0 :   std_srvs::Trigger srv;
+    8023             : 
+    8024           0 :   bool res = sch_parachute_.call(srv);
+    8025             : 
+    8026           0 :   if (res) {
+    8027             : 
+    8028           0 :     if (!srv.response.success) {
+    8029           0 :       ROS_WARN("[ControlManager]: service call for parachute deployment returned: '%s'", srv.response.message.c_str());
+    8030             :     }
+    8031             : 
+    8032           0 :     return srv.response.success;
+    8033             : 
+    8034             :   } else {
+    8035             : 
+    8036           0 :     ROS_ERROR("[ControlManager]: service call for parachute deployment failed!");
+    8037             : 
+    8038           0 :     return false;
+    8039             :   }
+    8040             : }
+    8041             : 
+    8042             : //}
+    8043             : 
+    8044             : /* ungripSrv() //{ */
+    8045             : 
+    8046          30 : void ControlManager::ungripSrv(void) {
+    8047             : 
+    8048          60 :   std_srvs::Trigger srv;
+    8049             : 
+    8050          30 :   bool res = sch_ungrip_.call(srv);
+    8051             : 
+    8052          30 :   if (res) {
+    8053             : 
+    8054           0 :     if (!srv.response.success) {
+    8055           0 :       ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: service call for ungripping payload returned: '%s'", srv.response.message.c_str());
+    8056             :     }
+    8057             : 
+    8058             :   } else {
+    8059          30 :     ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: service call for ungripping payload failed!");
+    8060             :   }
+    8061          30 : }
+    8062             : 
+    8063             : //}
+    8064             : 
+    8065             : // | ------------------------ routines ------------------------ |
+    8066             : 
+    8067             : /* toggleOutput() //{ */
+    8068             : 
+    8069         131 : void ControlManager::toggleOutput(const bool& input) {
+    8070             : 
+    8071         131 :   if (input == output_enabled_) {
+    8072           6 :     ROS_WARN_THROTTLE(0.1, "[ControlManager]: output is already %s", input ? "ON" : "OFF");
+    8073           6 :     return;
+    8074             :   }
+    8075             : 
+    8076         125 :   ROS_INFO("[ControlManager]: switching output %s", input ? "ON" : "OFF");
+    8077             : 
+    8078         125 :   output_enabled_ = input;
+    8079             : 
+    8080             :   // if switching output off, switch to NullTracker
+    8081         125 :   if (!output_enabled_) {
+    8082             : 
+    8083          20 :     ROS_INFO("[ControlManager]: switching to 'NullTracker' after switching output off");
+    8084             : 
+    8085          20 :     switchTracker(_null_tracker_name_);
+    8086             : 
+    8087          20 :     ROS_INFO_STREAM("[ControlManager]: switching to the controller '" << _eland_controller_name_ << "' after switching output off");
+    8088             : 
+    8089          20 :     switchController(_eland_controller_name_);
+    8090             : 
+    8091             :     // | --------- deactivate all trackers and controllers -------- |
+    8092             : 
+    8093         140 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    8094             : 
+    8095         120 :       std::map<std::string, TrackerParams>::iterator it;
+    8096         120 :       it = trackers_.find(_tracker_names_.at(i));
+    8097             : 
+    8098             :       try {
+    8099         120 :         ROS_INFO("[ControlManager]: deactivating the tracker '%s'", it->second.address.c_str());
+    8100         120 :         tracker_list_.at(i)->deactivate();
+    8101             :       }
+    8102           0 :       catch (std::runtime_error& ex) {
+    8103           0 :         ROS_ERROR("[ControlManager]: exception caught during tracker deactivation: '%s'", ex.what());
+    8104             :       }
+    8105             :     }
+    8106             : 
+    8107         120 :     for (int i = 0; i < int(controller_list_.size()); i++) {
+    8108             : 
+    8109         100 :       std::map<std::string, ControllerParams>::iterator it;
+    8110         100 :       it = controllers_.find(_controller_names_.at(i));
+    8111             : 
+    8112             :       try {
+    8113         100 :         ROS_INFO("[ControlManager]: deactivating the controller '%s'", it->second.address.c_str());
+    8114         100 :         controller_list_.at(i)->deactivate();
+    8115             :       }
+    8116           0 :       catch (std::runtime_error& ex) {
+    8117           0 :         ROS_ERROR("[ControlManager]: exception caught during controller deactivation: '%s'", ex.what());
+    8118             :       }
+    8119             :     }
+    8120             : 
+    8121          20 :     timer_failsafe_.stop();
+    8122          20 :     timer_eland_.stop();
+    8123          20 :     timer_pirouette_.stop();
+    8124             : 
+    8125          20 :     offboard_mode_was_true_ = false;
+    8126             :   }
+    8127             : }
+    8128             : 
+    8129             : //}
+    8130             : 
+    8131             : /* switchTracker() //{ */
+    8132             : 
+    8133         246 : std::tuple<bool, std::string> ControlManager::switchTracker(const std::string& tracker_name) {
+    8134             : 
+    8135         738 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("switchTracker");
+    8136         738 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::switchTracker", scope_timer_logger_, scope_timer_enabled_);
+    8137             : 
+    8138             :   // copy member variables
+    8139         492 :   auto last_tracker_cmd   = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8140         246 :   auto active_tracker_idx = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    8141             : 
+    8142         492 :   std::stringstream ss;
+    8143             : 
+    8144         246 :   if (!got_uav_state_) {
+    8145             : 
+    8146           0 :     ss << "can not switch tracker, missing odometry!";
+    8147           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8148           0 :     return std::tuple(false, ss.str());
+    8149             :   }
+    8150             : 
+    8151         246 :   if (_state_input_ == INPUT_UAV_STATE && _odometry_innovation_check_enabled_ && !sh_odometry_innovation_.hasMsg()) {
+    8152             : 
+    8153           0 :     ss << "can not switch tracker, missing odometry innovation!";
+    8154           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8155           0 :     return std::tuple(false, ss.str());
+    8156             :   }
+    8157             : 
+    8158         246 :   auto new_tracker_idx = idxInVector(tracker_name, _tracker_names_);
+    8159             : 
+    8160             :   // check if the tracker exists
+    8161         246 :   if (!new_tracker_idx) {
+    8162           2 :     ss << "the tracker '" << tracker_name << "' does not exist!";
+    8163           2 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8164           2 :     return std::tuple(false, ss.str());
+    8165             :   }
+    8166             : 
+    8167             :   // check if the tracker is already active
+    8168         244 :   if (new_tracker_idx.value() == active_tracker_idx) {
+    8169          11 :     ss << "not switching, the tracker '" << tracker_name << "' is already active!";
+    8170          11 :     ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    8171          11 :     return std::tuple(true, ss.str());
+    8172             :   }
+    8173             : 
+    8174             :   {
+    8175         233 :     std::scoped_lock lock(mutex_tracker_list_);
+    8176             : 
+    8177             :     try {
+    8178             : 
+    8179         233 :       ROS_INFO("[ControlManager]: activating the tracker '%s'", _tracker_names_.at(new_tracker_idx.value()).c_str());
+    8180             : 
+    8181         233 :       auto [success, message] = tracker_list_.at(new_tracker_idx.value())->activate(last_tracker_cmd);
+    8182             : 
+    8183         233 :       if (!success) {
+    8184             : 
+    8185           0 :         ss << "the tracker '" << tracker_name << "' could not be activated: '" << message << "'";
+    8186           0 :         ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8187           0 :         return std::tuple(false, ss.str());
+    8188             : 
+    8189             :       } else {
+    8190             : 
+    8191         233 :         ss << "the tracker '" << tracker_name << "' was activated";
+    8192         233 :         ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    8193             : 
+    8194             :         {
+    8195         466 :           std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    8196             : 
+    8197             :           // update the time (used in failsafe)
+    8198         233 :           controller_tracker_switch_time_ = ros::Time::now();
+    8199             :         }
+    8200             : 
+    8201             :         // super important, switch the active tracker idx
+    8202             :         try {
+    8203             : 
+    8204         233 :           ROS_INFO("[ControlManager]: deactivating '%s'", _tracker_names_.at(active_tracker_idx_).c_str());
+    8205         233 :           tracker_list_.at(active_tracker_idx_)->deactivate();
+    8206             : 
+    8207             :           // if switching from null tracker, re-activate the already active the controller
+    8208         233 :           if (active_tracker_idx_ == _null_tracker_idx_) {
+    8209             : 
+    8210         102 :             ROS_INFO("[ControlManager]: reactivating '%s' due to switching from 'NullTracker'", _controller_names_.at(active_controller_idx_).c_str());
+    8211             :             {
+    8212         204 :               std::scoped_lock lock(mutex_controller_list_);
+    8213             : 
+    8214         102 :               initializeControlOutput();
+    8215             : 
+    8216         204 :               auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8217             : 
+    8218         102 :               controller_list_.at(active_controller_idx_)->activate(last_control_output);
+    8219             : 
+    8220             :               {
+    8221         204 :                 std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    8222             : 
+    8223             :                 // update the time (used in failsafe)
+    8224         102 :                 controller_tracker_switch_time_ = ros::Time::now();
+    8225             :               }
+    8226             :             }
+    8227             : 
+    8228             :             // if switching to null tracker, deactivate the active controller
+    8229         131 :           } else if (new_tracker_idx == _null_tracker_idx_) {
+    8230             : 
+    8231          17 :             ROS_INFO("[ControlManager]: deactivating '%s' due to switching to 'NullTracker'", _controller_names_.at(active_controller_idx_).c_str());
+    8232             :             {
+    8233          34 :               std::scoped_lock lock(mutex_controller_list_);
+    8234             : 
+    8235          17 :               controller_list_.at(active_controller_idx_)->deactivate();
+    8236             :             }
+    8237             : 
+    8238             :             {
+    8239          17 :               std::scoped_lock lock(mutex_last_tracker_cmd_);
+    8240             : 
+    8241          17 :               last_tracker_cmd_ = {};
+    8242             :             }
+    8243             : 
+    8244          17 :             initializeControlOutput();
+    8245             :           }
+    8246             : 
+    8247         233 :           active_tracker_idx_ = new_tracker_idx.value();
+    8248             :         }
+    8249           0 :         catch (std::runtime_error& exrun) {
+    8250           0 :           ROS_ERROR("[ControlManager]: could not deactivate the tracker '%s'", _tracker_names_.at(active_tracker_idx_).c_str());
+    8251             :         }
+    8252             :       }
+    8253             :     }
+    8254           0 :     catch (std::runtime_error& exrun) {
+    8255           0 :       ROS_ERROR("[ControlManager]: error during activation of the tracker '%s'", tracker_name.c_str());
+    8256           0 :       ROS_ERROR("[ControlManager]: exception: '%s'", exrun.what());
+    8257             :     }
+    8258             :   }
+    8259             : 
+    8260         233 :   return std::tuple(true, ss.str());
+    8261             : }
+    8262             : 
+    8263             : //}
+    8264             : 
+    8265             : /* switchController() //{ */
+    8266             : 
+    8267         247 : std::tuple<bool, std::string> ControlManager::switchController(const std::string& controller_name) {
+    8268             : 
+    8269         741 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("switchController");
+    8270         741 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::switchController", scope_timer_logger_, scope_timer_enabled_);
+    8271             : 
+    8272             :   // copy member variables
+    8273         494 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8274         247 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    8275             : 
+    8276         494 :   std::stringstream ss;
+    8277             : 
+    8278         247 :   if (!got_uav_state_) {
+    8279             : 
+    8280           0 :     ss << "can not switch controller, missing odometry!";
+    8281           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8282           0 :     return std::tuple(false, ss.str());
+    8283             :   }
+    8284             : 
+    8285         247 :   if (_state_input_ == INPUT_UAV_STATE && _odometry_innovation_check_enabled_ && !sh_odometry_innovation_.hasMsg()) {
+    8286             : 
+    8287           0 :     ss << "can not switch controller, missing odometry innovation!";
+    8288           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8289           0 :     return std::tuple(false, ss.str());
+    8290             :   }
+    8291             : 
+    8292         247 :   auto new_controller_idx = idxInVector(controller_name, _controller_names_);
+    8293             : 
+    8294             :   // check if the controller exists
+    8295         247 :   if (!new_controller_idx) {
+    8296           2 :     ss << "the controller '" << controller_name << "' does not exist!";
+    8297           2 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8298           2 :     return std::tuple(false, ss.str());
+    8299             :   }
+    8300             : 
+    8301             :   // check if the controller is not active
+    8302         245 :   if (new_controller_idx.value() == active_controller_idx) {
+    8303             : 
+    8304          35 :     ss << "not switching, the controller '" << controller_name << "' is already active!";
+    8305          35 :     ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    8306          35 :     return std::tuple(true, ss.str());
+    8307             :   }
+    8308             : 
+    8309             :   {
+    8310         210 :     std::scoped_lock lock(mutex_controller_list_);
+    8311             : 
+    8312             :     try {
+    8313             : 
+    8314         210 :       ROS_INFO("[ControlManager]: activating the controller '%s'", _controller_names_.at(new_controller_idx.value()).c_str());
+    8315         210 :       if (!controller_list_.at(new_controller_idx.value())->activate(last_control_output)) {
+    8316             : 
+    8317           0 :         ss << "the controller '" << controller_name << "' was not activated";
+    8318           0 :         ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8319           0 :         return std::tuple(false, ss.str());
+    8320             : 
+    8321             :       } else {
+    8322             : 
+    8323         210 :         ss << "the controller '" << controller_name << "' was activated";
+    8324         210 :         ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    8325             : 
+    8326         210 :         ROS_INFO("[ControlManager]: triggering hover after switching to '%s', re-activating '%s'", _controller_names_.at(new_controller_idx.value()).c_str(),
+    8327             :                  _tracker_names_.at(active_tracker_idx_).c_str());
+    8328             : 
+    8329             :         // reactivate the current tracker
+    8330             :         // TODO this is not the most elegant way to restart the tracker after a controller switch
+    8331             :         // but it serves the purpose
+    8332             :         {
+    8333         210 :           std::scoped_lock lock(mutex_tracker_list_);
+    8334             : 
+    8335         210 :           tracker_list_.at(active_tracker_idx_)->deactivate();
+    8336         210 :           tracker_list_.at(active_tracker_idx_)->activate({});
+    8337             :         }
+    8338             : 
+    8339             :         {
+    8340         420 :           std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    8341             : 
+    8342             :           // update the time (used in failsafe)
+    8343         210 :           controller_tracker_switch_time_ = ros::Time::now();
+    8344             :         }
+    8345             : 
+    8346             :         // super important, switch the active controller idx
+    8347             :         try {
+    8348             : 
+    8349         210 :           controller_list_.at(active_controller_idx_)->deactivate();
+    8350         210 :           active_controller_idx_ = new_controller_idx.value();
+    8351             :         }
+    8352           0 :         catch (std::runtime_error& exrun) {
+    8353           0 :           ROS_ERROR("[ControlManager]: could not deactivate controller '%s'", _controller_names_.at(active_controller_idx_).c_str());
+    8354             :         }
+    8355             :       }
+    8356             :     }
+    8357           0 :     catch (std::runtime_error& exrun) {
+    8358           0 :       ROS_ERROR("[ControlManager]: error during activation of controller '%s'", controller_name.c_str());
+    8359           0 :       ROS_ERROR("[ControlManager]: exception: '%s'", exrun.what());
+    8360             :     }
+    8361             :   }
+    8362             : 
+    8363         210 :   mrs_msgs::DynamicsConstraintsSrvRequest sanitized_constraints;
+    8364             : 
+    8365             :   {
+    8366         210 :     std::scoped_lock lock(mutex_constraints_);
+    8367             : 
+    8368         210 :     sanitized_constraints_ = current_constraints_;
+    8369         210 :     sanitized_constraints  = sanitized_constraints_;
+    8370             :   }
+    8371             : 
+    8372         210 :   setConstraintsToControllers(sanitized_constraints);
+    8373             : 
+    8374         210 :   return std::tuple(true, ss.str());
+    8375             : }
+    8376             : 
+    8377             : //}
+    8378             : 
+    8379             : /* updateTrackers() //{ */
+    8380             : 
+    8381      142109 : void ControlManager::updateTrackers(void) {
+    8382             : 
+    8383      284218 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("updateTrackers");
+    8384      284218 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::updateTrackers", scope_timer_logger_, scope_timer_enabled_);
+    8385             : 
+    8386             :   // copy member variables
+    8387      142109 :   auto uav_state           = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8388      142109 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8389             : 
+    8390             :   // --------------------------------------------------------------
+    8391             :   // |                     Update the trackers                    |
+    8392             :   // --------------------------------------------------------------
+    8393             : 
+    8394           0 :   std::optional<mrs_msgs::TrackerCommand> tracker_command;
+    8395             : 
+    8396             :   unsigned int active_tracker_idx;
+    8397             : 
+    8398             :   {
+    8399      142109 :     std::scoped_lock lock(mutex_tracker_list_);
+    8400             : 
+    8401      142109 :     active_tracker_idx = active_tracker_idx_;
+    8402             : 
+    8403             :     // for each tracker
+    8404      996465 :     for (size_t i = 0; i < tracker_list_.size(); i++) {
+    8405             : 
+    8406      854356 :       if (i == active_tracker_idx) {
+    8407             : 
+    8408             :         try {
+    8409             :           // active tracker => update and retrieve the command
+    8410      142109 :           tracker_command = tracker_list_.at(i)->update(uav_state, last_control_output);
+    8411             :         }
+    8412           0 :         catch (std::runtime_error& exrun) {
+    8413           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: caught an exception while updating the active tracker (%s)",
+    8414             :                              _tracker_names_.at(active_tracker_idx).c_str());
+    8415           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the exception: '%s'", exrun.what());
+    8416           0 :           tracker_command = {};
+    8417             :         }
+    8418             : 
+    8419             :       } else {
+    8420             : 
+    8421             :         try {
+    8422             :           // nonactive tracker => just update without retrieving the command
+    8423      712247 :           tracker_list_.at(i)->update(uav_state, last_control_output);
+    8424             :         }
+    8425           0 :         catch (std::runtime_error& exrun) {
+    8426           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: caught an exception while updating the tracker '%s'", _tracker_names_.at(i).c_str());
+    8427           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the exception: '%s'", exrun.what());
+    8428             :         }
+    8429             :       }
+    8430             :     }
+    8431             : 
+    8432      142109 :     if (active_tracker_idx == _null_tracker_idx_) {
+    8433       40477 :       return;
+    8434             :     }
+    8435             :   }
+    8436             : 
+    8437      101632 :   if (validateTrackerCommand(tracker_command, "ControlManager", "tracker_command")) {
+    8438             : 
+    8439      203264 :     std::scoped_lock lock(mutex_last_tracker_cmd_);
+    8440             : 
+    8441      101632 :     last_tracker_cmd_ = tracker_command;
+    8442             : 
+    8443             :     // | --------- fill in the potentially missing header --------- |
+    8444             : 
+    8445      101632 :     if (last_tracker_cmd_->header.frame_id == "") {
+    8446           0 :       last_tracker_cmd_->header.frame_id = uav_state.header.frame_id;
+    8447             :     }
+    8448             : 
+    8449      101632 :     if (last_tracker_cmd_->header.stamp == ros::Time(0)) {
+    8450           0 :       last_tracker_cmd_->header.stamp = ros::Time::now();
+    8451             :     }
+    8452             : 
+    8453             :   } else {
+    8454             : 
+    8455           0 :     if (active_tracker_idx == _ehover_tracker_idx_) {
+    8456             : 
+    8457           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the emergency tracker '%s' returned empty or invalid command!",
+    8458             :                          _tracker_names_.at(active_tracker_idx).c_str());
+    8459           0 :       failsafe();
+    8460             : 
+    8461             :     } else {
+    8462             : 
+    8463           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the tracker '%s' returned empty or invalid command!", _tracker_names_.at(active_tracker_idx).c_str());
+    8464             : 
+    8465           0 :       if (_tracker_error_action_ == ELAND_STR) {
+    8466           0 :         eland();
+    8467           0 :       } else if (_tracker_error_action_ == EHOVER_STR) {
+    8468           0 :         ehover();
+    8469             :       } else {
+    8470           0 :         failsafe();
+    8471             :       }
+    8472             :     }
+    8473             :   }
+    8474             : }
+    8475             : 
+    8476             : //}
+    8477             : 
+    8478             : /* updateControllers() //{ */
+    8479             : 
+    8480      151825 : void ControlManager::updateControllers(const mrs_msgs::UavState& uav_state) {
+    8481             : 
+    8482      303650 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("updateControllers");
+    8483      303650 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::updateControllers", scope_timer_logger_, scope_timer_enabled_);
+    8484             : 
+    8485             :   // copy member variables
+    8486      151825 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8487             : 
+    8488             :   // | ----------------- update the controllers ----------------- |
+    8489             : 
+    8490             :   // the trackers are not running
+    8491      151825 :   if (!last_tracker_cmd) {
+    8492             : 
+    8493             :     {
+    8494       80956 :       std::scoped_lock lock(mutex_controller_list_);
+    8495             : 
+    8496             :       // nonactive controller => just update without retrieving the command
+    8497      242868 :       for (int i = 0; i < int(controller_list_.size()); i++) {
+    8498      202390 :         controller_list_.at(i)->updateInactive(uav_state, last_tracker_cmd);
+    8499             :       }
+    8500             :     }
+    8501             : 
+    8502       40478 :     return;
+    8503             :   }
+    8504             : 
+    8505      222694 :   Controller::ControlOutput control_output;
+    8506             : 
+    8507             :   unsigned int active_controller_idx;
+    8508             : 
+    8509             :   {
+    8510      222694 :     std::scoped_lock lock(mutex_controller_list_);
+    8511             : 
+    8512      111347 :     active_controller_idx = active_controller_idx_;
+    8513             : 
+    8514             :     // for each controller
+    8515      668082 :     for (size_t i = 0; i < controller_list_.size(); i++) {
+    8516             : 
+    8517      556735 :       if (i == active_controller_idx) {
+    8518             : 
+    8519             :         try {
+    8520             :           // active controller => update and retrieve the command
+    8521      111347 :           control_output = controller_list_.at(active_controller_idx)->updateActive(uav_state, last_tracker_cmd.value());
+    8522             :         }
+    8523           0 :         catch (std::runtime_error& exrun) {
+    8524             : 
+    8525           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: an exception while updating the active controller (%s)",
+    8526             :                              _controller_names_.at(active_controller_idx).c_str());
+    8527           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the exception: '%s'", exrun.what());
+    8528             :         }
+    8529             : 
+    8530             :       } else {
+    8531             : 
+    8532             :         try {
+    8533             :           // nonactive controller => just update without retrieving the command
+    8534      445388 :           controller_list_.at(i)->updateInactive(uav_state, last_tracker_cmd);
+    8535             :         }
+    8536           0 :         catch (std::runtime_error& exrun) {
+    8537             : 
+    8538           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception while updating the controller '%s'", _controller_names_.at(i).c_str());
+    8539           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+    8540             :         }
+    8541             :       }
+    8542             :     }
+    8543             :   }
+    8544             : 
+    8545             :   // normally, the active controller returns a valid command
+    8546      111347 :   if (validateControlOutput(control_output, _hw_api_inputs_, "ControlManager", "control_output")) {
+    8547             : 
+    8548      111347 :     mrs_lib::set_mutexed(mutex_last_control_output_, control_output, last_control_output_);
+    8549             : 
+    8550             :     // but it can return an empty command, due to some critical internal error
+    8551             :     // which means we should trigger the failsafe landing
+    8552             :   } else {
+    8553             : 
+    8554             :     // only if the controller is still active, trigger escalating failsafe
+    8555             :     // if not active, we don't care, we should not ask the controller for
+    8556             :     // the result anyway -> this could mean a race condition occured
+    8557             :     // like it once happend during landing
+    8558           0 :     bool controller_status = false;
+    8559             : 
+    8560             :     {
+    8561           0 :       std::scoped_lock lock(mutex_controller_list_);
+    8562             : 
+    8563           0 :       controller_status = controller_list_.at(active_controller_idx)->getStatus().active;
+    8564             :     }
+    8565             : 
+    8566           0 :     if (controller_status) {
+    8567             : 
+    8568           0 :       if (failsafe_triggered_) {
+    8569             : 
+    8570           0 :         ROS_ERROR("[ControlManager]: disabling control, the active controller returned an empty command when failsafe was active");
+    8571             : 
+    8572           0 :         toggleOutput(false);
+    8573             : 
+    8574           0 :       } else if (eland_triggered_) {
+    8575             : 
+    8576           0 :         ROS_ERROR("[ControlManager]: triggering failsafe, the active controller returned an empty command when eland was active");
+    8577             : 
+    8578           0 :         failsafe();
+    8579             : 
+    8580             :       } else {
+    8581             : 
+    8582           0 :         ROS_ERROR("[ControlManager]: triggering eland, the active controller returned an empty command");
+    8583             : 
+    8584           0 :         eland();
+    8585             :       }
+    8586             :     }
+    8587             :   }
+    8588             : }
+    8589             : 
+    8590             : //}
+    8591             : 
+    8592             : /* publish() //{ */
+    8593             : 
+    8594      151825 : void ControlManager::publish(void) {
+    8595             : 
+    8596      303650 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("publish");
+    8597      303650 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::publish", scope_timer_logger_, scope_timer_enabled_);
+    8598             : 
+    8599             :   // copy member variables
+    8600      151825 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8601      151825 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8602      151825 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    8603      151825 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    8604      151825 :   auto uav_state             = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8605             : 
+    8606      151825 :   publishControlReferenceOdom(last_tracker_cmd, last_control_output);
+    8607             : 
+    8608             :   // --------------------------------------------------------------
+    8609             :   // |                 Publish the control command                |
+    8610             :   // --------------------------------------------------------------
+    8611             : 
+    8612      151825 :   mrs_msgs::HwApiAttitudeCmd attitude_target;
+    8613      151825 :   attitude_target.stamp = ros::Time::now();
+    8614             : 
+    8615      151825 :   mrs_msgs::HwApiAttitudeRateCmd attitude_rate_target;
+    8616      151825 :   attitude_rate_target.stamp = ros::Time::now();
+    8617             : 
+    8618      151825 :   if (!output_enabled_) {
+    8619             : 
+    8620       25399 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: output is disabled");
+    8621             : 
+    8622      126426 :   } else if (active_tracker_idx == _null_tracker_idx_) {
+    8623             : 
+    8624       15083 :     ROS_WARN_THROTTLE(5.0, "[ControlManager]: 'NullTracker' is active, not controlling");
+    8625             : 
+    8626             :     Controller::HwApiOutputVariant output =
+    8627       30166 :         initializeDefaultOutput(_hw_api_inputs_, uav_state, _min_throttle_null_tracker_, common_handlers_->throttle_model.n_motors);
+    8628             : 
+    8629             :     {
+    8630       30166 :       std::scoped_lock lock(mutex_last_control_output_);
+    8631             : 
+    8632       15083 :       last_control_output_.control_output = output;
+    8633             :     }
+    8634             : 
+    8635       15083 :     control_output_publisher_.publish(output);
+    8636             : 
+    8637      111343 :   } else if (active_tracker_idx != _null_tracker_idx_ && !last_control_output.control_output) {
+    8638             : 
+    8639           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: the controller '%s' returned nil command, not publishing anything",
+    8640             :                       _controller_names_.at(active_controller_idx).c_str());
+    8641             : 
+    8642             :     Controller::HwApiOutputVariant output =
+    8643           0 :         initializeDefaultOutput(_hw_api_inputs_, uav_state, _min_throttle_null_tracker_, common_handlers_->throttle_model.n_motors);
+    8644             : 
+    8645           0 :     control_output_publisher_.publish(output);
+    8646             : 
+    8647      111343 :   } else if (last_control_output.control_output) {
+    8648             : 
+    8649      111343 :     if (validateHwApiAttitudeCmd(attitude_target, "ControlManager", "last_control_output.control_output")) {
+    8650      111343 :       control_output_publisher_.publish(last_control_output.control_output.value());
+    8651             :     } else {
+    8652           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the attitude cmd is not valid just before publishing!");
+    8653           0 :       return;
+    8654             :     }
+    8655             : 
+    8656             :   } else {
+    8657           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: not publishing a control command");
+    8658             :   }
+    8659             : 
+    8660             :   // | ----------- publish the controller diagnostics ----------- |
+    8661             : 
+    8662      151825 :   ph_controller_diagnostics_.publish(last_control_output.diagnostics);
+    8663             : 
+    8664             :   // | --------- publish the applied throttle and thrust -------- |
+    8665             : 
+    8666      151825 :   auto throttle = extractThrottle(last_control_output);
+    8667             : 
+    8668      151825 :   if (throttle) {
+    8669             : 
+    8670             :     {
+    8671      120730 :       std_msgs::Float64 msg;
+    8672      120730 :       msg.data = throttle.value();
+    8673      120730 :       ph_throttle_.publish(msg);
+    8674             :     }
+    8675             : 
+    8676      120730 :     double thrust = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value());
+    8677             : 
+    8678             :     {
+    8679      120730 :       std_msgs::Float64 msg;
+    8680      120730 :       msg.data = thrust;
+    8681      120730 :       ph_thrust_.publish(msg);
+    8682             :     }
+    8683             :   }
+    8684             : 
+    8685             :   // | ----------------- publish tracker command ---------------- |
+    8686             : 
+    8687      151825 :   if (last_tracker_cmd) {
+    8688      111342 :     ph_tracker_cmd_.publish(last_tracker_cmd.value());
+    8689             :   }
+    8690             : 
+    8691             :   // | --------------- publish the odometry input --------------- |
+    8692             : 
+    8693      151825 :   if (last_control_output.control_output) {
+    8694             : 
+    8695      254748 :     mrs_msgs::EstimatorInput msg;
+    8696             : 
+    8697      127374 :     msg.header.frame_id = _uav_name_ + "/fcu";
+    8698      127374 :     msg.header.stamp    = ros::Time::now();
+    8699             : 
+    8700      127374 :     if (last_control_output.desired_unbiased_acceleration) {
+    8701       96740 :       msg.control_acceleration.x = last_control_output.desired_unbiased_acceleration.value()(0);
+    8702       96740 :       msg.control_acceleration.y = last_control_output.desired_unbiased_acceleration.value()(1);
+    8703       96740 :       msg.control_acceleration.z = last_control_output.desired_unbiased_acceleration.value()(2);
+    8704             :     }
+    8705             : 
+    8706      127374 :     if (last_control_output.desired_heading_rate) {
+    8707       93318 :       msg.control_hdg_rate = last_control_output.desired_heading_rate.value();
+    8708             :     }
+    8709             : 
+    8710      127374 :     if (last_control_output.desired_unbiased_acceleration) {
+    8711       96740 :       ph_mrs_odom_input_.publish(msg);
+    8712             :     }
+    8713             :   }
+    8714             : }
+    8715             : 
+    8716             : //}
+    8717             : 
+    8718             : /* deployParachute() //{ */
+    8719             : 
+    8720           0 : std::tuple<bool, std::string> ControlManager::deployParachute(void) {
+    8721             : 
+    8722             :   // if not enabled, return false
+    8723           0 :   if (!_parachute_enabled_) {
+    8724             : 
+    8725           0 :     std::stringstream ss;
+    8726           0 :     ss << "can not deploy parachute, it is disabled";
+    8727           0 :     return std::tuple(false, ss.str());
+    8728             :   }
+    8729             : 
+    8730             :   // we can not disarm if the drone is not in offboard mode
+    8731             :   // this is super important!
+    8732           0 :   if (!isOffboard()) {
+    8733             : 
+    8734           0 :     std::stringstream ss;
+    8735           0 :     ss << "can not deploy parachute, not in offboard mode";
+    8736           0 :     return std::tuple(false, ss.str());
+    8737             :   }
+    8738             : 
+    8739             :   // call the parachute service
+    8740           0 :   bool succ = parachuteSrv();
+    8741             : 
+    8742             :   // if the deployment was successful,
+    8743           0 :   if (succ) {
+    8744             : 
+    8745           0 :     arming(false);
+    8746             : 
+    8747           0 :     std::stringstream ss;
+    8748           0 :     ss << "parachute deployed";
+    8749             : 
+    8750           0 :     return std::tuple(true, ss.str());
+    8751             : 
+    8752             :   } else {
+    8753             : 
+    8754           0 :     std::stringstream ss;
+    8755           0 :     ss << "error during deployment of parachute";
+    8756             : 
+    8757           0 :     return std::tuple(false, ss.str());
+    8758             :   }
+    8759             : }
+    8760             : 
+    8761             : //}
+    8762             : 
+    8763             : /* velocityReferenceToReference() //{ */
+    8764             : 
+    8765         809 : mrs_msgs::ReferenceStamped ControlManager::velocityReferenceToReference(const mrs_msgs::VelocityReferenceStamped& vel_reference) {
+    8766             : 
+    8767        1618 :   auto last_tracker_cmd    = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8768        1618 :   auto uav_state           = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8769         809 :   auto current_constraints = mrs_lib::get_mutexed(mutex_constraints_, current_constraints_);
+    8770             : 
+    8771         809 :   mrs_msgs::ReferenceStamped reference_out;
+    8772             : 
+    8773         809 :   reference_out.header = vel_reference.header;
+    8774             : 
+    8775         809 :   if (vel_reference.reference.use_heading) {
+    8776           0 :     reference_out.reference.heading = vel_reference.reference.heading;
+    8777         809 :   } else if (vel_reference.reference.use_heading_rate) {
+    8778         717 :     reference_out.reference.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading() + vel_reference.reference.use_heading_rate;
+    8779             :   } else {
+    8780          92 :     reference_out.reference.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    8781             :   }
+    8782             : 
+    8783         809 :   if (vel_reference.reference.use_altitude) {
+    8784           0 :     reference_out.reference.position.z = vel_reference.reference.altitude;
+    8785             :   } else {
+    8786             : 
+    8787         809 :     double stopping_time_z = 0;
+    8788             : 
+    8789         809 :     if (vel_reference.reference.velocity.x >= 0) {
+    8790         554 :       stopping_time_z = 1.5 * (fabs(vel_reference.reference.velocity.z) / current_constraints.constraints.vertical_ascending_acceleration) + 1.0;
+    8791             :     } else {
+    8792         255 :       stopping_time_z = 1.5 * (fabs(vel_reference.reference.velocity.z) / current_constraints.constraints.vertical_descending_acceleration) + 1.0;
+    8793             :     }
+    8794             : 
+    8795         809 :     reference_out.reference.position.z = last_tracker_cmd->position.z + vel_reference.reference.velocity.z * stopping_time_z;
+    8796             :   }
+    8797             : 
+    8798             :   {
+    8799         809 :     double stopping_time_x = 1.5 * (fabs(vel_reference.reference.velocity.x) / current_constraints.constraints.horizontal_acceleration) + 1.0;
+    8800         809 :     double stopping_time_y = 1.5 * (fabs(vel_reference.reference.velocity.y) / current_constraints.constraints.horizontal_acceleration) + 1.0;
+    8801             : 
+    8802         809 :     reference_out.reference.position.x = last_tracker_cmd->position.x + vel_reference.reference.velocity.x * stopping_time_x;
+    8803         809 :     reference_out.reference.position.y = last_tracker_cmd->position.y + vel_reference.reference.velocity.y * stopping_time_y;
+    8804             :   }
+    8805             : 
+    8806        1618 :   return reference_out;
+    8807             : }
+    8808             : 
+    8809             : //}
+    8810             : 
+    8811             : /* publishControlReferenceOdom() //{ */
+    8812             : 
+    8813      151825 : void ControlManager::publishControlReferenceOdom([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand>& tracker_command,
+    8814             :                                                  [[maybe_unused]] const Controller::ControlOutput&               control_output) {
+    8815             : 
+    8816      151825 :   if (!tracker_command || !control_output.control_output) {
+    8817       40483 :     return;
+    8818             :   }
+    8819             : 
+    8820      222684 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8821             : 
+    8822      222684 :   nav_msgs::Odometry msg;
+    8823             : 
+    8824      111342 :   msg.header = tracker_command->header;
+    8825             : 
+    8826      111342 :   if (tracker_command->use_position_horizontal) {
+    8827      111342 :     msg.pose.pose.position.x = tracker_command->position.x;
+    8828      111342 :     msg.pose.pose.position.y = tracker_command->position.y;
+    8829             :   } else {
+    8830           0 :     msg.pose.pose.position.x = uav_state.pose.position.x;
+    8831           0 :     msg.pose.pose.position.y = uav_state.pose.position.y;
+    8832             :   }
+    8833             : 
+    8834      111342 :   if (tracker_command->use_position_vertical) {
+    8835      111342 :     msg.pose.pose.position.z = tracker_command->position.z;
+    8836             :   } else {
+    8837           0 :     msg.pose.pose.position.z = uav_state.pose.position.z;
+    8838             :   }
+    8839             : 
+    8840             :   // transform the velocity in the reference to the child_frame
+    8841      111342 :   if (tracker_command->use_velocity_horizontal || tracker_command->use_velocity_vertical) {
+    8842             : 
+    8843      111342 :     msg.child_frame_id = _uav_name_ + "/" + _body_frame_;
+    8844             : 
+    8845      222684 :     geometry_msgs::Vector3Stamped velocity;
+    8846      111342 :     velocity.header = tracker_command->header;
+    8847             : 
+    8848      111342 :     if (tracker_command->use_velocity_horizontal) {
+    8849      111342 :       velocity.vector.x = tracker_command->velocity.x;
+    8850      111342 :       velocity.vector.y = tracker_command->velocity.y;
+    8851             :     }
+    8852             : 
+    8853      111342 :     if (tracker_command->use_velocity_vertical) {
+    8854      111342 :       velocity.vector.z = tracker_command->velocity.z;
+    8855             :     }
+    8856             : 
+    8857      222684 :     auto res = transformer_->transformSingle(velocity, msg.child_frame_id);
+    8858             : 
+    8859      111342 :     if (res) {
+    8860      111342 :       msg.twist.twist.linear.x = res.value().vector.x;
+    8861      111342 :       msg.twist.twist.linear.y = res.value().vector.y;
+    8862      111342 :       msg.twist.twist.linear.z = res.value().vector.z;
+    8863             :     } else {
+    8864           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not transform the reference speed from '%s' to '%s'", velocity.header.frame_id.c_str(),
+    8865             :                          msg.child_frame_id.c_str());
+    8866             :     }
+    8867             :   }
+    8868             : 
+    8869             :   // fill in the orientation or heading
+    8870      111342 :   if (control_output.desired_orientation) {
+    8871       94529 :     msg.pose.pose.orientation = mrs_lib::AttitudeConverter(control_output.desired_orientation.value());
+    8872       16813 :   } else if (tracker_command->use_heading) {
+    8873       16813 :     msg.pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, tracker_command->heading);
+    8874             :   }
+    8875             : 
+    8876             :   // fill in the attitude rate
+    8877      111342 :   if (std::holds_alternative<mrs_msgs::HwApiAttitudeRateCmd>(control_output.control_output.value())) {
+    8878             : 
+    8879       88451 :     auto attitude_cmd = std::get<mrs_msgs::HwApiAttitudeRateCmd>(control_output.control_output.value());
+    8880             : 
+    8881       88451 :     msg.twist.twist.angular.x = attitude_cmd.body_rate.x;
+    8882       88451 :     msg.twist.twist.angular.y = attitude_cmd.body_rate.y;
+    8883       88451 :     msg.twist.twist.angular.z = attitude_cmd.body_rate.z;
+    8884             :   }
+    8885             : 
+    8886      111342 :   ph_control_reference_odom_.publish(msg);
+    8887             : }
+    8888             : 
+    8889             : //}
+    8890             : 
+    8891             : /* initializeControlOutput() //{ */
+    8892             : 
+    8893         228 : void ControlManager::initializeControlOutput(void) {
+    8894             : 
+    8895         228 :   Controller::ControlOutput controller_output;
+    8896             : 
+    8897         228 :   controller_output.diagnostics.total_mass      = _uav_mass_;
+    8898         228 :   controller_output.diagnostics.mass_difference = 0.0;
+    8899             : 
+    8900         228 :   controller_output.diagnostics.disturbance_bx_b = _initial_body_disturbance_x_;
+    8901         228 :   controller_output.diagnostics.disturbance_by_b = _initial_body_disturbance_y_;
+    8902             : 
+    8903         228 :   if (std::abs(_initial_body_disturbance_x_) > 0.001 || std::abs(_initial_body_disturbance_y_) > 0.001) {
+    8904           0 :     controller_output.diagnostics.disturbance_estimator = true;
+    8905             :   }
+    8906             : 
+    8907         228 :   controller_output.diagnostics.disturbance_wx_w = 0.0;
+    8908         228 :   controller_output.diagnostics.disturbance_wy_w = 0.0;
+    8909             : 
+    8910         228 :   controller_output.diagnostics.disturbance_bx_w = 0.0;
+    8911         228 :   controller_output.diagnostics.disturbance_by_w = 0.0;
+    8912             : 
+    8913         228 :   controller_output.diagnostics.controller = "none";
+    8914             : 
+    8915         228 :   mrs_lib::set_mutexed(mutex_last_control_output_, controller_output, last_control_output_);
+    8916         228 : }
+    8917             : 
+    8918             : //}
+    8919             : 
+    8920             : }  // namespace control_manager
+    8921             : 
+    8922             : }  // namespace mrs_uav_managers
+    8923             : 
+    8924             : #include <pluginlib/class_list_macros.h>
+    8925         109 : 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..a8c4942e35 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.overview.html @@ -0,0 +1,2252 @@ + + + + + + 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 + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + 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..af76f04a105298f084e74fe69980e4a922b8f044 GIT binary patch literal 26131 zcmV)OK(@b$P)mV2i=$MwX7(7U)?@5k3j9lNN>71<6r*kxs>f&{Mh$3? zM>U{+xegzr(0hB=GC9iD%pQXdAG^g_*)tl@&^1Rp52zCgJ)mwc>{e44WA8jej3aPi zjN^uhFb)~$JU&+EniaE=Dn-WCF-AScu{MQ_k>w*dQVFnqoN9~`U^dPYVB0uZj2tSl zYc*gqBPERbG1AUUd>l4L3o+__re|m1Fn++Av7F)<_6UF->IY%85Tm!v+6VVZV;pBp z1dPXhkjJ4`%5qwkMdM)m1zPGo&jLKvVFTb&lmCdZX7WWqFFc|grGGq@S0BJqCI zOK^ZaqXce_;0~4BgCqJ_Kk=ynja@GUr3g5(?+G&l#kvGIyl2#P5&ML$k4_@}x*3CU z$5#j3iB@17PxV19e+qf9HI;~&n%g~shCC)=YPZ8bW(w7S8g9&+>tXJc&zYG==f}!E z7ngz(a8YAC4Fl9RXdMmmARU1nKIAePY;a{u%79TzDq@enA3vc#k9hGNfsy&VF~X4+ zZC3!O$0)-mUew2^)9((Gf|g2$_IBah`upFufAW7G&a|!J-{5a;mmE-NhjaSH&3_jW zN`L{L_RIhhjIj>@2)=TF@)!vq`c>LBB?iJC$S)SyqXnFG@CmDbgwl=LOmTA7VrL>5&E+{G6y{OUF@m`c;tIYRp?ku9Fk+0y@P9BtEn{T9gjAn^ z%FkL?&=ykme&d8b+XiBSR$3ryj_EQsjI-g0sOUO%q_y^T$m0TRhT|@K$5Re?Ivj(a z&i#hd1`ObGbi^LL90R4>L`*qYf$W6KG3ctO{<$%Fx*?#>V-CkyzhOVn^L1e_^!ry` zGB{$CfQ2X<0?5PifIV^?mdO!pFG92ujA}GWl-M+f=3WGBq6w2w z+h-CW9T^1lABbI-aXtP_Ozh9H0XoX1B);speyY zbHa{|1N4j*0GCL{QpBikyx@A#ZCd1L74CaJIMa+h+ULg#>lo7oX|JXLn8Mj7d`AEm zV7G$va`=rh)f#K0O0}mKdH{Q=4#r*FZn4@Drx1>((SsDu0w?H*Xg^c{gE-tX5oH7k zbfXs5Aq6a~Y||LSIpQQQQQCD<_O&sp0T(Tx$2k;`=OzzW$5`GzR8*@Qwo;C9A@*1) zV6Z2LmGy%~8WID0{g23x3w3BaF@=_P@zSoa;~54RNl;*Ptpb>69X3Wa#%4fqW(Z(u z3KtU}Uk7NPnWaEA1hg#&&TrPaUqft!WE~)rhrj}6W8(nDWT_0Jo)`<&bOb%b6#PAG z*IGdC>W%}{cdf;kf%1To^NIlK=UR)g=MLbI0;Pa&N-btQMs|#9K;@dT0OtIWFp5M~ zTl7z#k`>Ma%KhL`$YK#e)A)nC=QMG-HJ0G`qQLcM7#JA`vZ(kOx#_d_JJM zL20Zz_X#h2)9nfwtO%&@x>ch^*LDp>ffzL)?xM_|4r1z;J_8uuGsc>P;jT#|e#;u5 z8Y4hi0X#+c8ekiH+}&lB3W80aj>9XooAa2XZisj$TEfDhI${)UkNCMSZ7Q9ni$z;Zgp zxo_vAH;>|U9Az69aea|pp9dmqg0G3lVW)^WV&Jjklw92d*ps{I2{H7}Ou|?<)dj$Q z>7fjl0S|QHcVE!9^07e75sdC2VziCXjqn)mXN0qB%UmCid)!QaWWgzf1{H>wjZt;I zKV*pbHl-vtV03XBRe+)o9!QKrgjB@f7Fbo;G|>^J$Fg_6inLeyi?$3f#g#>7#^Itd z=58xL=_n)z0w$7JK(Z8^7_qLV+*A)3)7UNSLF<1=-UqtUfLAlV7du9@{znWF3nGdg zq=4-24T_!(TPqavw%t_~5}1it=(vFlj4^VsdwQ}7k+{tPcJ2xpHNE%EC$e*O4bNdz z`zeCTTf$}s?)Kr7XSyf*d}s=tcHAG{L^wwAk&XSiqs)ViiLr++E|p5T$Q3yALB6+SzgItF;)Nr+YO4;Ks7m>olqZb)?V-=WO1-o zKDxQKbREZExdx0Md`H45&QDPN9Gqd~sbc{-Xc>@Ed))Uuo>ykz+%Wqv@>fY^z|LG! zaF_u;AUnovU;?uCA^Si*&SRF84Q zAOi~{FVkq5uH#?}1I@)u&{G^>8xfEgK*&)<2)S3r2pOK7XB{KOV_d>1RtV*Y9||32 zGb4?YVfbTG^2y<5Mw&%C)y1POoS8~I#Q~-D_s>lhg}lI_bU1wLzX+**fs#^r-%esX zYJQ!?NboK~)IbI($zmq8MPl$CG#s#7n}OMN9SgfK1?*3E5o}y7U|-h+rR3VDfMSG3 zd)LxcKnB>ICyq)NT#a6V!z?#_nt3!Tb~yXUD{?VaL{Y`E886f^pQ5u~`t zi+~H*!cZR!r{X;MUt|C!yC=tp7i0nOpasM$w?qv3H#FvqJwP_g3iyjNQ4r;ZTtx zGmb*E!>Lu-4@xlB;D|kn42*!}H@Yv`ZH^(tAQ2F8vSVzhYo=w#*^|(Y7|hglqgzMD zI3XnFfKKe9MC=qK_f84p3D0!_P}jv+L25{1Ii|2{cW<}EA8FV5sz#1b>#XKpSN}7eO4?Z* z7z2oZ0|-aC>ysaRhH=RN7vHskQZh!J(nEmS3V!t$o>XHzlBtgSqGzWaqty1pFqi-l zFpk9oHW3rfxln8yFS>(YW(merTt-115G%#pjkfDk&>P8up#~CNxy@aBgFcob60go< zSDb<3$Jo$yQk1O2Lh71y-$-?_@2qR=@`hb!dts*7Y|U^GZ`~%U7!wsiVMO1B731hZ zB4;evtc%3tLenHY6u}6WU8MfEvYx%n7 zb!5jX+^{eRfWQB)1Jq*N^j$|$%K_V{4w9R*QjhV#+VR}x?h=B+;Wi%aqp<=2l^C@x zQk|_U+~!doTjs-k0@Yyg#1H5#UoF-7Gs55nW9D^W0WyKIfGdR(F*4Lxh>0Eje&URT zh>`0%-#QOxrEa!NVIr|KNNJ4BbIcRRxNZf@RT?9wduH;nI7o?)jNLO~jAA2Qi*TD3 zu=+eWkF*Sk`KV=2INHOP3-D`15_?vLW`2yTo;{2O+toGbK6cRzxWRuTU7Hvl9Na0G zGO~XOH|%ANn+DPeh{@WUi5TLFUvQX1PnDSR?+V0Z+aY6o@T#M48|Jt<2qT!D5+Imy zlRWCD=%fq&koH19NML-Z*^@AC@_}@^1yDlXK+KjX;+kO!4=oF=JcVQqk#`JJEdpZu zQO_Qav2<2&fV#=nQ+>Vu{M>(NyWQ_s+3qL>p_X7d1rS=O*I}ri*F=k_8 z0Hra;QD%UH#%K^TG^^V#KUeNuRzUXdg|HgV@QVU!g4 z`S#e^?w_rs2wam;JmvxzKV)j!XJ*4c$Em5#%>C}Rk++>0#Bdc{lYYb#LzhhZ%;1Sh zOQt$AVPf8J&E&33eSX5GOl)qnngpx|x zE`vg1t5X=5r7^iS4Io%EYQQlP?wZal^g|d%H2@q*%%<%6q8+22Jq~aTF>0$hbUaQ; zLBL@%v#Pqjs81i@;L|@wtp57k4~%*?-?5K$Eb>$XQ@6w({Y5v+M-dQ%j#~;A)vP;T zhXHhpSr*3fhJe|G%YfuwM+BtTko|!AF=l|;DU78jn@xq`e59cyI)i;{p_;9)VK9(} zJBAGF+P*7*%g^4qCQ}$)Jz_AYc8o&~;yQ*aU7w`mDYv8}Ez*4yqr2XehUPlblF}{( zunGrsO&sUL_!zA{Ek^n}hY#%FmMe1bY_2zWRq8DO=^|%8;QBuvS2+iNG_9U<-_J3W zharyf_h^kcq5u?dZj1xOjO>LXoPKvjGNipO=v_0~I8rh`PfR}_skcyz(H=AG(><4% zSm`025;J@mlbb?v#8cI+<`AuNd`9w*OP*ofTo0uB7*%sW)fqK($Pd6R`zMB5@WuxW zodYyb9aEE!rx@h#B03AiNO=u*gMG1f4ZBA}-~eInP{SCdDLj)5tCDI~-r6ew1EsBi zO;mHiW{LI7>wTt}T(2IRnc z?p@`KMNO2=ew2DjU>de~3Vh55*)lVwjLJtdCGS2K-?~whD`vhqde)DKB#)`AR7_Llc$)lGG z#2{LJ%ESm52N7dPmC!ENT(noRQo~ZvE-pdi5{28Ad_niW43Ik$_8cl=c3SlYq-^$_ zJc_+*<#KIckLL?}FwOw+QEw|?Yy)g0MwOIXS9{U7H#|$@tW3&PsOGrOOdMk^-?cKv z1UQqAr9mqAICG5qAlJ2MNZC|My9MwVi5m~T-Mf@+<``wyW&zx?t-ri!-ojoPH_T*h%;l4^{@QOD+ z&=-sSfr_7^XQR@brpWg{8Z~6y3+1{LaIV2x=Tp-?;HE*yt{HR>=(4%6W&gHo#z~JC zTUg`N|JNWry+T19NwbAwLr3bdLgD_qyPtgi%%=ziBNgo`rBf)F!U0o-qM#^xh2oC2 z#K-Ke4W+EmMws@Fnf`E?nN^-^Rg;xebI4$uk}61Kx01>v@+0JtTo)8i!>TsTN~u}l z;>_f#;v;6J-vV$m8a~WlTuxv3-Y8)I;zDM?XG~$!pS25}VH#Npp1sIb~%Yf$4^mFi>Kbrdv4=_ZWBR~h5 zWpsz60XyKvb?qyJP>dOG0@e(01mXkag=y&1C)WfEtUYYmDqD z#WCK zMLuSL|LK}x*ujlab=K5(P4+^%siti>j0NhsWP!6)GiMywV`|jpi#2Fv9c`$>)-8aJtg8zPSLz{ip_PBIcV5Fm{aJT!6kY zzA6{MFvddC07xD5fW<`Ha5ePwqydlSt&;})Hy^*T6J@Uz+yUWF;BxT^v4wI7^qBA` z7y|tfLeW2kBphh^6wV6|jW>p$6(0Isg&)5Pv+DEx4lt5Jl9I;5d<-{3=qD<#EY4(Ya7EO zr+t+}@c9J{UoY{D^f-VHua8Trha!E^$M8~6acmZC=10g<(UhD>{wF7nUGfNpA|InX8U4h#6`1JgAqF`W*6f414m)|^85Ixht)t0Sezfut z&y4$$vj>tqg5NFzLNTZSj_z6wc-8_!lx09A#<@hWp2W62}k8 zVW~z`%CQ$jrSOIa1_YCnevt#*JVtQ}Y1dUik5Rt+xWHlMuKNbtEDK=c&-qczN*4w$ zTJa7d4dVy)j@`p>A4{} z>bUR4269#q)h<0s)d2ZN0}8~*8s9p`ILZQ`mOWtmh#SWdxFA|9&nY~YfIZ?mSTjCK z3uaFT#^3O2BEzT$6jpzx_WXc&j5fgy9WYb9;xTR&ndox(N}+BLq=-=y2)K1VXrY+G z!XPzcTpdR37|F{(cx8<_KniMw+hLhd4=@OaSWxAWqVdjFehEEfKvCtF`$#-7e+-=@3o_6#e^_`rHsW1 z3>SCRIx*WNi{!|C_7XYoMhp{{@{Ys|6SIff5ytq;nZdKe6T{37do{$(&U1;O&u)pt zT!=QySi;WElV^rxv2ny~8e;fW^Gss4;!;45c+qS?&AfaSK%JONJ|MPhWfOd)44~Q$ zSGOs4xVLhTgK#r8ZoCA&fTi7J_Y}jW-EmKGlABrKv}}iO6YTKtI`BS^l`*bKT-o9I zuHhWY&WcTe@p5s+15sX-3UL2MJT75_UBKx4xOzTD_fmGsqU4{41xTjz#ESwmA zqV7QkSd+t-?yf_lkjr7Vu)*{Vp9G4)(PIj5^ZNma6K;c^9BWC3TCL5Y1M`m+*D z2C7TWPd5#HyDPol-z32NQpHRRH+BZ_JvR2BA5losbDl4wd?$P~GrY7AGPSp|bNy(& zM%+_8>@9Ph4j=5}9rNZoWf!0t$6>||r;p(UlMca~GR$Lnu2)tviy32R3W?f8!1=|1 zwvTm?1;7UbGk~^wQhd>H$@LFi zMuU4A`Iz1Ir5D&AA4E+NDsEO{Q~ShBD41lS`n2oN{XT99_i{bA)B$6>;`71r80CYQ z{Q_xw5Roa2SnhD_*;jX6fvFy5vC>Zx9Ntqb2UuwmQ>DVHp4#aJPI z7NBN~kM>yYRq@`9?3ny5+vKJPe`cBNp}Psv!lj^Zmj;{KAAnxBkMo!?`0%@BtZCWYy_jm1*sSBotC_18^a_(O)E%vu9^iU zz9JW+fKlkWbBwq>098(8tCD)CDSGqpzbL5@UE8dbuYUaMM`j9#18Uk=Ki2i*_#>$% z>T4)_k_TxF#u;OrH-#z(N$xDTxHIIsC&~Nt?#EAu&kBkScQf9-`P3_4MiNuBfkk>U zESFxDa&bWkbwCrweQ;Z;{CpGDa`@V%DW2k2eoNsGh~X7LW!I?^TJ>l>{(hwQXMj>300i{EoB+8D2m)9cqvUzavkNvBu)!7YCwok78fFSPrza(5B%q$^S@noU zIHMj7Q_cBX--hbC@SIcVWqFKK0Cns+=UZ1N##sy>C1Q`3__!65V_N|KrIq>b%FL{3FCM`%NXBVD9&F)^J{2QKtmg?qlC)>hM2cX zs=}pyJ}s%LR^ZH>hm`#`w?A~;0tCz%pjIf}K7RG^DU}czT+F()U{p%@x3;3HwxS!S zqGaO5T@DF=D=2WdCz5NcH9ogwA}GsK%yLGXCiq?`DXUN`&nSBzSic1*To?X$mW~Z0w;UuB_4-TL2s&h!~;+ zVjTvXws^-o*>z1QKoS<9?lB>%7&eb3L^Qs$^Af=G@{=GXzse`Y?|Qt5crs_ z@i}}nV|*!l3g@^!FPGyu{$L6nc(Ig@XVIz1PcfKl_xCv%X6S&nCbQRefDB$M z;J$Ku85}u^YGl|R1j@c&yG?Tm8*7XXY}&8U%a@`7RR91W07*naRFYx$9&o6@Evu?y z@!128CuRq_LD#udu)L3eX9>_;PDZ^J&Po%&e%oA?$aAt4gtql`ofog6RIeA&nUQA3 zn<>xgmj8J#sWE?R@fP`4)76K3#O>} zGhKg@rZ8Y!OQ`lQS8Lt}=SJWF)fkBlutxD*6GH$-(0%@U7lTklWaVYM18P&rw4ym0 zPTx!yV)0SuPz@|@#ZON;VQS!YUp8%Y`uomY>@&h=-#4RbDd5IZnPJ3!v*@b3`~Q3u z)%%aZZ>F9ZLydDSA9uC4Zo*#Fh|zECwd>Rx9jWV!WTX&4UAo)V7s%z?J*L*~URU%l zXxe?H+`DtnswUeq*U81siSN3FSHG1FZAck2^DJNW%+zZH3L** z6nD)n?w$}Mg2QaztefVJOfyE!;(D}K?V7LMUAfIQoG0hLIG#`*V~y&BuzSB=v%`h8 zjNU`(kvk}Qo78$MC84^ziUt;pS2#vcVBwYlsxguqv0GI7_x=B_EPBHDIfeI42a z^ivqR){IfR&%AOK9+QBdG7;~Q@S!gQ<*qok{F@phb040M_irCbh0V3&ND2^!KX!Yy z31c1bRVPjLQpm-OT@zn1t7BByaJ=W^LKwCOa~h`btg+$#p64UkJxj^%@fFUGDdfXn zLbxCRUnt#>3Uj8jweriR&HVrb{IMn* zT>0_&pEvs$57AxbMJ)u7BHmKRP}*4X#bsDC;n|%SLBUPnmF=5buG+5mKQmf#l({9f z*9eWD$8_yIVCbR?xWh8D4EL{E(z&EX2hx$xe^(YHR5nUAyx+U||KAbS=r?k^#*J~@ z0Da3BY>0~|OV@{Y1PBq~ST^R))SWYem?`AedMb$LMA-*&8RMH^Q>W0|vIe$TseLv!qH~;|VF){&%P? z;z!dy1NOAVCp|C%a*f+exd?RM-A6j_+~e7*6{8NYit%IaK0>6O`#U@m+8B@PEiRN7 zldLe`0WpTz1isalbuPJP_N~wf#O>}PUDQIM5edtAW;upC+_obG!|!=n=fmpvaDfD2 zj-hAMaPZMrLCCfg0dPX{Zrmi~4P*}vfoyqUj<}p6-jsh0X=DIjo4vnhL^DTal2vJ? zx)zalkS`0tec>g9_`gHe`5LNsaKtFP*3Y%F9ro}?bQd0D+8d{^?d5j&Gg)KMg_LfN zKqz}ym&DhUGDks9%#k>y!rlRYV~ICR1f#q;OmlPbd2#u|C`G2i{rg@3bo=_^F8$Nyc#`0Li0 zxqz2D&>w_7C8|}tIj2;zaEnkLBRcvFNsD2~&T1G!z$(V6Yle=Dj8WA!#TY_iXr4wp z#!xngzBpo%o(y9!^|F~!jqwmnfOMWd2}b@0+@?;fm^P|pT7Gs--@uY-6V9&hP~VJ_ zykeQRP#xDx#K|9W;fj9EX;`joW_9?|LWnY;4WkCofbpe10LEn(z6e=r|EaT}#-6~T zSF-1OY94ZGPQ^AV3-KVIJ-qR=x$kZIQFAQJB2$H9VM`gkghK*UW9&R3F&HPDkfb?; z=SRyF4!`kLvXN#>iwDgaj>=}imE!%489DqEZt$qtfUyerH%lCU4fdXG_=V;*EFUcR zGmWX=2art>ibNeb{U%)^qaQa*{xyj!2(c2(6e=6Vcn^QYz%(Ch8(!4@p^eIlUMR5r z$VW7F5yKBq1muXBVv&Z>-r$a37A}-Q$VAQrs6C`Q#Mk9iH!|D@F-8H3Gun*tA>r&4 zm@!r-a@t%gCi`PG#VrOi-2q@5#$j`<0h~0~$Lg}?@2~TzZeG{YsnE9 z$CwBBC-gTE_Q^z4@{nx_bicpOB^H>8L1X;=36`=*!dnIeAIpFMlnXe8LKORM4`IFe z0Ssf0w<|j_rdZma{SzC(+Y`7;_O@1xx5pe*0qtXa{P39SDj<@HGN1;dXUH$$Kw{(* z00z9>WX(Sn@P4fVY8tQ2x-*;Hn);oR!oFz4_*h=QB;Q@o`T&fH<6tu@Z>CrZKpVnf;gndQys=@ZVe|jl>WOTm698~c)9Wz zL$>7Ip|>0XM&G-fkw6OT=MoEzPXW9X!T>I9m6ftmGr3TBl^{QHk^pzs%(4QNwSb|?5;Tq z*kO)9|Hbzs|r0xWh#80~3whJE@r(axWfD$|QTZw@fMRWND=(!TyUjl*8fkz{l6p5-}#O1Ka(vz z=|yc2&AFhz_K$s)zRHi^y^!Ib5Y?3U2%T0;#k6Q^21RM*SEwuH9i5 z0eMq2P9Z`w3)?ATZ0NcIXu>1){RtB2Pdw0!;p!>OV{sBsWI(=NYERb!B>^Kdi&<;x zT8ME8fb;f3fzgO+UDvOZ9_o^cCHe9izRhCBGw2h=Oq3-f}`g7{pFv;i+oPg3UNa;<8(} zQ2u2F{o1HL0L5KiN$3qB@EpM@R>^++uzR^sgwBZ4L6~jp{@>~o$>U@sz~aR@0<3qf zAEUZpQbtXv-$O5G3p60W7hdlphD#h}Vz~88Btfe;c9E zbZpKJz)yN*Jr=P750Ew;#K8@u`PZ4oU9tTTfA{U@es8)>`ChRx#AK}HeRd|tIlPvR zj|p|iS)Y%zhZvIEjPBtmMwR#+^ku>4zBhb2(&zUYW`oZ;);bSV<<&Fy4b&ZYKs_ntRH! zyT!e0obfG=QP=gAqvr91ho>o2z<~7tc!|w1mRE#vjL51eQ7zBSJp)vAJ^G<+eFG96 zDlA3uNT-J@6^bD>3k0D^rLXR9CKN@gIcjjNwvQ1C9Y#nrR`@_t?tMbRHxuC7S8ABz z`iRxAy(t=_ratkw7lny08q|mpp3=u)jAN55>;&llNwTnMkPV41Y#Ae}nw$iqCweY= zn__)F{x`3D@u!UL91v43SW+I*J%`Hy5z_(i96;U7TwRSi)@!um^MBhg-fGY8Uiod9 zT@`Zx#o9-uF(OaJ%|C7Y_zSa#GLylmC|o*z`_MQ1wtyst6RzRfPFP=AH~?&$>HDd2`-gjck+R z2mKv;T`~^(M_@kSYABESARj!7>Q_VF0sJ;sL*Utn&a0uATx9-jrlksOk0uvu34mJy zl%jFaYAIn(G^wmG4& zoyyMjGL-^p0aKi@-WcponqPy*V|snfKuKcwnhkdUX0?v-Oln@ORKOo4D@_u9CeP5r zanY`ak8$?9&7rUvC*kU@sZ=Dr9In!i7NNM}GeFv!$hIN_;v?#<0I0_30Q@)#&(%A? zTR5E70dMzMkF*3Bz8pe~mOVMcUl=*(HMlL|yx^pSQB+km5JWFMzZvs>6$b>>T)C>`dZeZF!?B^q}&+`u!Tsp%7Jd zZ_XvXMZ7&F>KxUJH-O_a+-_e)RNv^pZ#N>34-fUrFOfrJ^by`bK_!oL}9zAA;UD&^H*%U&~{;)sXopWEuMLQ*qE(A0oGjVVxB z7?~hV^I{detKQq1`!{Csf?^hOSWD_hdI*}n>1*O6sp2XEsxk8Eg+Ri(Fy5tyck`~3 z&dmUa$C2L2G8H)4w_G6`?+LI4qhq5qMwX2X;LJcEufWs#+P`em;o;V<eU@j~`TzSd zvS~5-m+RLvJjXNaL%&(}XsO=qo_N0Y?yYpbh<1il zGQ7AB&QZ)N_r2$mJZ@3y1K`KVV>!kav!`B%y}+KP<@(3MdyYeIe3^|sPn6o`Z0zsw zCdu=k=*_Ntx88i$4C9`;n_ZJv%L(* z9QMm9KT5S=92B^#QOpFeu(tZU&brS4dBtp9*V=%5g<>|V6i^gIta0eNt|?}A=i-}Q zrKs7XZeWD+u6~|rKQ7@Qm7?u)5`93v56YKLN#`^2bM1di#L#(R4fhYpl}bm_V~U1} zXVL+sg9LlHD@U9piElBZTZgf9(hODVACB6OIcbWCMZg|t*5UbkasHzf;~IV)u9rq- zzhhK5k+HH*0+a1^&XUq}eR$m~S)bud&0nhODn*4;<(YBo&agu4&;RH=L~Z?RI+f~h4{?S-IX{PyjK zJ_-~7l~jktb+0Tj1X#gn>l!KMTlSuf8DqI#vIm?g6oXVe}$q5Xb_5I z36uNLMxi)IqqZCom(*~l=z%bU}-OGye0ka zg=5;#vAi86&+EPEky3$eVqP1rffZ` zAnY8&fOR=LC|@tDbRDa@3SEQzKGe02J>xGHl%VlK8R*x0w1RIB z{lni?q+_09YEq;jrScZyb()?!{wmTUpg|}EMe4O-P^CSJv^!tnt4N155)v{%o7OH# z!oU7uMOx6Pjh9N;6Jz^R5BOo|z7^jv{-+`7M$T zc!H_cV~n^40b08Li$43A`3huFJlfw?Ql$$txUP$XKnmbqw!?J-Ij@jzi0og&^Q|$L z#w<~!Sfj|K0O)_bWtAi;V8AFUa;rAuu6(iNTP0j~<*UR9UIt+X+K$mIxDOxb4fSd6 zdfI8&b|i(*kllQZ>=1V{X&e5N`^=c@;r}&tCp#;z@#NT6B}V(;PdsUIWzsI;7K3XMtDaH%54H*#`^|9EgzL0UzgDxsx$6a(U1SRFJcU{ ze+V{>tUNQL%Q~uC$kGNbsPq|p8_$Jt6YviQQ7a^F!{r*RPx0BqND}xj95IfT+dC4Y zNuk$B&pSoMgKY;Ya(h&~rV6VLB^C}Ev_?-PZkEiJ2qotI-7UP*KbIk*4cJ_2k zfts69XqI|=qo?K?EOsx8S)BwRJt~1LW@+3p;LEZ!zB!O(jHLcizftqQ)&l^>OpzEp z)f3E_4QVE@GI~65n#HA3qfP87>f8T$U5_pEp<36EHfvqG{YZL-E$lNEOX@|JpLWl% zTtLV1QO$6$XxOJinlRG9S=!7PRfae*KCbRj%b)xdlvyd#Rir+R^^B~&^Io1@;v-^( z+-X5Bi3U{UBF~@VH{?R)?^Unt^zJ{QrTGtNXalmpE zQu(n)GRD{n21|pO`3;QQw9I1mDu$!ynAxZNQrC$Lj?-s5;8wo^IQw&9*t%{^UHR}e zc5`cUpN{iJVyy6)J4)WM6k&j}cKeWFdt$Jdy3ev^6m%F_p5M>F3P z=jA|#N;WYal9t^39Kpryb0b}FiqDY5*u<6!U<1aVxGgb5g~VJR`CJk?7~_~C5egm> zz%qya-(1qc=3c1`Gc+ImmbItU%%s@~o?Yw0`MqT?7;p(nAPW`vm-VK!BkVRv%-BZ~#O+^DT~5 z1XQ;t#HDnLx~d+)<Y9 z^?#CZEaG1YFSln%c&|BLlkh*ABhE{BMvR{7uY`|i-}vw`65buxOyiM_>?y?|+YdhH zz-@MwW`NsyLp^aJtO>C8{H?MUiL%Wt9$1Q~{*1V0AKz1l`;eM0gS`s1hhRhdTQ`tI zo5>2zMP3QjP$QWS_B4d;q{YRouU9TEPj#eOj#W@LTyh&qTnP{`y^ImHjxpZDe8eGg z65M=0QwR=8fQdUbj1TS!IR&uAC`IwMO|&VXD^n zVTX$QPpB!Isg>`cqSEv9Lq&fHmamJ%pv^#p0(o3>po?TPGu#dGtIha^%}2>W^3`V5 zo7zZ0QJCps!B{)A+;Z*!sQ>r9`nm-4+&w5?Kozh z)AMC=A=b?%xX-G$m%~c+_WbWbKrJz!P;U=aYD!8Yyzst%THP4qJcro0HP&L zs4j!w@iAx$UV3_}7SgdUZbWb&mLiqQLM7 zSgY53`O9Ip(*wyv``WnW_O)@#?Q7$HZCnb7mO^s<_@V?UcHwVDN`GzKGT;Z;xNN|w zC}yc$uf%&N0p@|w_lbZ?3BO)>k1zc4cFiJFzcwzb0KPV^gz*Il?@fE5My1$n+&8ZZ z{9@ZI$VHKsiro_j_BOt~OK{l**WMOmyU^|$A=rl)yauR0Ry)~cx7!ITH%tq|`BxLT zXA_hd$sh}WZ5YKf0IlK=&H$A$LK*I*T6~SoxC+n8Q-#%UR$pBf+l&}KH|EUngRPXJ zSur7#M3f$~cKZE~7jg!GPmL6f_6bjooRzz#g=(F%IRiWk&SvH%7Tz^H2T)J7#@USC z`zTHJ15h>D2fZ~s_1)&n1}u?`n9H9shL6Dnb{5(aKkeVzJN$cT2!F?g>|6PDq6;v& z;?Yn5)H575TtPwt3@VhJApHLI7Jr2SG>lOyZ*;+c28(6zY5!5+R_T)BO^ zQyNbFcZ-{{t_u%kf2cfmO62zx_ZK8QZ6B9Q(jy>#xMj_3act5wS^+PK+6o}_nO9Gm zl5^byDx152xIC^ORy-s51q3r+X<7l`lxvs2yt~dc*unPt-CgHc%_4_BL>|330_W>7 zIAJ@zt|Lk%JX%r0*CgZL{kq)VX3gllt{&vK=Q>6>#4<)q^m73>!o~z>;N#abECZ_B zCuE(Mk11RH=dao;w~tkOVbn4p9AX*c<*W7>_I%Y|ZyR46tLmLu&9PixwU@2e)Fygo znAHlpYMern=~13Sk*l9Igyc+K_G^YlGeM-98F{>h|3} zzS{?F-|gf5r_js2;~dX$#M~!rAD6-hj*b8TAOJ~3K~zzIHDa>uqb2F|yL}V@)ot?j zF~r?m+dkHYi^cEu!7gmi$FwlN+sAuOp%;9|IiBH&xu3LsQ~@{P^8UMhlmS0(`xvr? zXhMq~TgZ3&D7WwS@!dWufRC>}XZygv_T4^efFH7b6x)&j@$a2TWd{JL#AqJlGB6s) zOR^J=d|~*%8*WLr>N`Gaj9}3fRPJ8r=k5~cZ=qh#9sCs(XI5hfl=?ws|D(qB|M(;6 z@h2AMTgit&^-m!6Ce(GxD=UGj^^mq#QvE44T~5k+dt%iI`0a~p+1+a8=~(G`&ov{k zxVc2#!R4;k-@j7Vvb)CDOR$!5OYn9NuMB_0a{ZQJt^n$(X2l178Ot!U;&Z!)>U6vO zFt050;*%=RRmA7@mFFbhw1}D0;Yai*yTs{v=92g7yDmyZOPrLNkKkC%NMQ?mh=gx} za#y=Q{pT@qvWvg-Bb6-TCZQ-g6C|n&9-X%3y0X@Xo#E$YlL2JwCir_k74(mB5)fCY}OH6>ko&wxtk$?R(r~7{m142e>cfyMnM5EW3iP9n?pF zvrrI#7BH9ss4dqzWs7smHPq;q02`O9csCXR9qEMyi!lBI%Is+cBuxO!99&>u>|NJ% zRe#s@>~)>J=OX}anF@aOa&?Zn#an#8rXE*WpVvJ3e$rD~4XoNSAYD4= zc061782=xmnmATsl5A7hcbh`|0pHt{%3ObMQ!?`1#~twcy-i{HV`X^jZOT5o7u>7w zx+oDJcAMgYgmzoA)-|VZ*>_WTP}&AwY3%>-z0zP5K-g%$R~ib8xZw7XlG{+KHe#Hl zANF;f9b=+gBQaC97|h8Cd3XycECuD7Nx%LlZ&Pj%JpA&cFtTt+czitWr-m+R3uqpQ zj{Dhz_wiBN3m?o28^5?J{8bD#@wBYOs*D; zsAV@k6NIAHHY|}|2cpSRG0bqgiTW%qn4OlzBuGl!gfZR_F~cinQ)66Cm`Wt=Kvqo z7|Cs7x9JLQ_nHn^93LlKh^1$*Dq#|7mo9kYj1pe(KWq(v`mVE`Yh_q!(>?ilPrRv6 zV0^UXwF`lqOa5rVb@!3{u{-S5doqO0DP?8jEuf`-Mijn^xYJ?sQvv;#r^G+ zzq=7nM!+L{P?T_;;0Cf&x5xMqjnB@erlDFJYap*n8yeQdE+WF*%^NFw-ZY=q2sNkD=ra< zzEK|+W>(gM zi4`NJg{#5B0{;Gt*D7ES`TE`us3#_u8m#}!7_Sgh#v1~t!&vQlyqe01L9-sYW(KTk zsX)shLJ~1(s@Y9$mc|`wvLkN)O4txf>V>ga!9|UmX)SDVp<`^g{^!Zgl}m zmjKkK(is0p9V$Rb;uPk#6Kwj2!ksjS0O%M2ihO(TFX76dCR47I z%5UDAk#nhJerAPd(Hq?aV=nJ4W0Z;(tN^Jslw(Go=1(ew0;CW~&vineq*`#NxXn(< z8=b4(uoOyBb{BMC_`-zF$@i{QY=M|DAimx>U0h0eJP>2KFvfgcty^xlr$Rp{8vp0! zj1VW8k3YLo8=o7HZ^{tMvXn7<3^N1zDOKh`VNzHQNY`*cD?h#fm}yQv$_@(Q3V%>w z>}1bN^Wu(Fh51`HI!TJ;IXA{vCeT%yIp)Kw3NaneD-CPJ5J>AU#C%u)!CHltA(-$d zku3_qylWlRFLq`)t>OR`>&vG4#;Q^!cOt{LPif-++qBWf~tszv)a*X zzTa#Ml+%S_3NA);Z}_JK_(5Ve^wk}vwjm=JHimmd>~mW^81Y<|0fEr8S9N>+krdqK z$zAiwK*WG7oM&*G;E7W&E;W0m76cizzSx>GeZlV9G=yD;E7bF!dt)((K4=`E86zAk zgSH7Hyi2Kcy^00Xx`-iDW51t~u(cOMr|jnW{X`v}F{r~6Cu~Yn?p>vhvoUJB-otQx z^{`W{Z64L8y~g>wJS($F_`2qr)n%I&aLQ#on8GlF(arsuDdb9wv@u+C5cfFF`G5%n z;JhhZK$zDF{Mqo*fRsy)${?j%fb`FJDuXA)Vrah1BDFKz*V_tLLt)VjWO#0{Lk`Oh zT^%3QfN#U_!1mJ#elg-LjSH{rKgkC$?xeyWQ!Xf5+zW-0J@0S?$Qlj2P|x+teE?E{ zKh(9_2cYeGPrdS6On8AOWkoNkYv~?`dm=-e9W>Yfx%$>u!m1rwuwwb!??sd&wRf@BN=K8;0*TbCl+`8tDAOGpP<}mVrU1RLBAJtvY-)H*8 zQk=njpWy{HRLqX$WK7hU-Uf8tN&Dhv5?pQ-=HDHB5gi4_8BV}85Im@AQQ~9qZM$Bd zeOLXSbYxtLhI8qqQzwvc3YpgN^vyYRP@eQ@Zv&=qH2Gq&%${hGlmbqX+45oRS;`Vr zlb%3(F&-C6(1)@|kRbKqs+3-cS#Xx*-SCU$%gu?|1*yKbu)3qV$BY zsRs7=g;SHBSl-|xq$f~acpU>wVJ#%yO<^4y#Haw&3dILxX#rz+)ai_My7c8~qN(QI zMq1Nr)xv!Nic)o5!(oy*=unL^Kicp41>6|rt|9LAQ#|ZG?rIdqOC}Y)wZoosp7B`d zWm6zWPA7MOM%@#Bu>kldbr}3?E-L zA(W^2@qwXg2PDLfWW?5ZM~*Hk2en?f{=?>U9{_>+Gz_LHWtJcD2gi!)f8 z!n6?IcM4Hv{QFO#A7Q;~<+;aj@R_l@LO*vb2j3n<63xPZvVX|Z1uY4yYBb_XrdKq% z>KLavrFp}(>CGlnRD9Y%z@q&@M#yVZplXgG$GYxC8D%S=X=-!v(LjV+_s9^pHsj2=Bo zVa@11n`-!uK69z(I+c=u&#yD=8GpsrD-_hJ`xT1OLQ$59iJNA*t=wF5w^`@^c6a4G z>_{?wv-rc)FP5B5(JZWE9BvkJ?@2%-7K~BZb@r&koqTa8g(6>I44bYCZrnpC#2IPz zKT1{X5uh65A5x^~aRnrSwCnd13R=_qD->lwyHJ#6VwQfCpFv8z&H2du)?;)Z;$eS6 zm_)KjVTF99?GbAh96>N=dOz{lnoF8OaB}0hVT|KTtKp;OCYhRTnekaB5(l_{S$WmA z2{5)kVhX3>HjKK1&sFIKe9Dnjirk-Cvmjl>h58}gtB{e&FpX1I;r=9{AN$ei6I4yZ z2lJrp{UMLQoHNPo#dwHS{IOol8LreR3GiGy@>Qq|+W%=%7IIk*f7&WSbKAa?~ z3d?&)+Qo(EI#ACM^Rgll#Ni97erH+dL{-S1ID52WLCF4KwBp-ad+M8tt6;Iyg*3gw zsc4$DWv7-*vj|wTxS9dw6=sZUuA637p{Sc?4HvX8k{(GNBVsR@3Q2-Qw9K-s$jT1Pm{{c9@S* zfYk%G6C=I?g``KOhG94ls|QrKGnMdwlDzi9ECuk8VO;Q=brnWr^11kx#}r!2?zE5} zoAx+u#S7dv!u9AP$!v)oE)$KGjKx)YywO@nH(=ape59Itt_eQUa$qxf=ZyXXYfAy2 zRG{ydY@R)@R8l^3X9-on_y-4a0{~YKw=XX6NLF-13LE?!yu2c>f|3u zOV9II#t5D^<)+ zZ776h0My-d=i#BrLdkTv7I0ov6P}u`M_{PG$IKqo9#ofO`_q~cr;v_uv~ypr_P`hs zo4RfR(oNV2yiCM|BKbF1U}DE|3SWhDgaZT*R4oAQG2aojR7<{0z>WEVK-u{tluZhN zs`&7!5kqoeV5?9-K{%d5x%x%!`VuWFP9F0yg4R05XoDv{oj7`=TEn*hcBkzDbTf}TM z!(Z#YhJh4|V^J2x3M(~~d>dojv#VD`s}-ZhpJBZvJ)dj-jATDn{27~hi{p9T^mqx` z3uEiq;wBvTG){%CDP)}r+!XTKs@=gx1AA5gsKzL1lnhY2bIm)k?=%V{w3!O8Hm}jC zS-XA6`}NDLl!;eB+e0XU6FPH6&y}%5c>%Kay7daDRXDnTZLqa&N zie2ZKO?hE@om7hbTqlbgfH>5j)QI)Cl~8`q8c}|&eF@|vz{baLUL&Mz(hZc zj%osakMa;7`Zj-^cT3y?PC#AP=UrJdB#@|_y|`)~?4BD}(}LXL@xQ38z0kCRlhJFX#?wyHATF5z@kTjr~ z>RA#F<`Dg{Px)?({BD}+?L!GS?bXIXhP7*O1<+u@wwMoFDb!SpKPKDV?(yS1o1$@y zp_+RP`+CY4ix;)x7~^MeslBoelG2~icNxENC<&nZl2d-xu1mLouGt;FS;k!c>W&&C zlQzI)#+aN}zq-dD8`X9N6}(CpmOJ3>{u5tgV0^#(*9{e_z;!A&f(Z{Xe25$8IO;gz z8Wuspe{7TRKflkobaSY}-*2DU|HHfCr1bLfIY+OvkXv+QK=qX zO~897O%M!}dlYO0NYrNR;SQCc@tMrJ9-UZ2LoqX2hR>=GVhY!b5+X~zXOc`-YJi^UU@3iY01_V{@B-`p)?_A;SJPMoq+{IMZuN!B~7$@A%{AKexRsp-`fSTRoQ_O44 z#ZVi=%kLL7vge+1aPO&f`H!<7U+emO`4ON##`m(#oL3=PX5S+~t-a+DARpuVi5Wg> ze#8*~hLL#3SSq946h>k=R0P922$U#2>aN$y4r3Jx=4xm##vx-|Qynxz*V5_CNK82i zxdmew)R0((;df03y8iv9laAu|0@QIJ+{+MI)n1ZgpA;x5p#_6;+ zJJgNVUi* zXYaG^R*q`MfsqsQy5(w`!pK-qb%B`~B{5h3E@7=$L(>y$%3}&BDn{@d$_{Cw-WK9iGANTgoF z556BhkDY0zWJw)&&NA6y)JCcwEf!WdhN>mZXkgb-7I3ez>%p}jV9%vkpoIJ`$@zDQ@?)t{!&8?Vi>} z!9_ASU?9E;xEnF|q!=tNl0)U5AhsDLZ<1ynG1upCsR>Ro{)-%tuHm@Kz(!*<*h%<9 z-_s7~n3GorBuhJTH3OV-5RYUJxRyS=hdh?F&j@+U>xVx<9tRN9CyzPRUwJGMgRaiy zF{+63J1;U!`|QNq`YJIbz^!*_;+lf$b&`S`q= zb$5P$#9TXDxL$tVY7DCYU*--Y2Y?jnd-q1W|6rh6Isgzaz(IKqHyp&LI@ks!Bw@9D zVcTk3g&a!enZ>qLveH&#^K#8lr2MwlhVk$o`^+eY=kJn<$34hM5#!Z8&p{sT$gd2- zl}3z{kTnI1G1tKqqo`8@#=1%|VXUINn$lw4L(B#`G;-xt6*KwD_bU{Y_90#cvZZmD zGu>0V=vKYO^q)>4bRm}{y*=@(AL9h}JkJ>Z2~p7Q%Dc@NE&_grF$`;$hjgm+z*JLd z-bv{B|NpEqxN8(m_-yE^)@%U-I~Tv@2wAoFEk{^tll>G!LV>Ud3W{`X z4%;aKeidoJ#-aQw(i+CEBCTNjDpDCEvPeg0QY?akBK_5*Uqy;mKew)x?0Lt!*Ahl4 ztJ$}&A}zP;SCI~Ba~0{BLVj2TSw*^w`d5)6mT>D@$)2x8noCy$G3T!$EiHxXSCJMm z!b(=@LA3B?tJig8k(kQc1uhraxMq^Z8JabuxI3!#{nhug`fKH?R6V!aJ`Z7_s~uI_QvHITXnZT)zrl_?T2Bs_F25Ya@8 zstiIaM#mS5{&%lA!bcG158_Huhu~nJN(z;=|RZ>osPvJDrmhgJ>vj;J;(9hDlB43b zCewzLkd*<=7|YL8*f8?Xg12Wt?2OU?sxjhiST=^K?(xqhq?%gZ?s~EA$u!E%+lMhS ze?O*ZRm#Ua1UtACswSX%fJK_E-4`nWyj%xhe9Pq;PDnmIee`mrKEQser&NHJGzF^R zVLy`AxT)7YpW(6_T9fCukziD35AqDx&J42&- zxQt$05-&bfU}sp8jLQXAQPY<#`;0>cjCYuLmR%hMu0DVBrBb1@?hWnjqYytKK+WJwjq_PlQ>Yj$fBIn7q3MLATQ+Cdcj1s`>ss|d7wS6n#neOH0X161Haq-d zLrXr;u__>^I-W_9vCXmvG|V2$%mmls5jwDsDY<0vT_b|5n56@t{x^wCm6pRTcrTDn4qWtu3xD zxVV;d1YJBa8$5FG4<3(RT=6{fHj9fk0H_$Fu_@-dOm+LrqyxTm!k<7j?wQP6*0ts? z&ek=vO6eH%8@sdi>W|eqYcKPr!iV&nS9J%}oSc=Sb1=X)zPlGl0T1r|q{nnJK=j~V ze7OCM-rbYjv!9h+FzVhOD;@3-=l3VDO?imDS1d8k9+6r@>yQjxa9F!_e0!@ z)4i^VJ)XpmVvl^U08V0!URTkjh-RiDy2Py*XD@TSew^1eeyFmCN^RrSId#th{}Axd z{ul1qj%=6bx@S|kfZJz;6EJR#K2q-+{fQs%QSJX>A8DpklACmbk2J#=gH~gl+lDqI zalVs08XEKsEUbex0WMxaEujX?ae=?<7eV;m`SJ=~cY22NSMu}>*LyUzfa5*GU&nc| z@s9JyM+!LEan8#{2FN^xWMq$1r2X!P@T)(6!u=3_`(xY>+4^Lp!mwOB?VcQ?x@%Fq zOMn|%2NZB}oIPTDE7cd3iThz5q+y94Z1Il1=SGvTaKaYbJ9v=9PGfv{^%l-S8_(CJ z7KH{{%*U7wkiG+eN{rf;9~6M{&vq`AYEHMsu33kax3{Qp3Pger`&J6YZiSZ(`asNn z#((%lpg68wWky#yD6EMXixHPXD3XlRqsWdouvzwUU!Gu_p}fj_47$IOrN9+9bv8{A zBQ${!;Jr{@)npc`+wfFvt5Enu>vBpGf@A{<1tSZOmIU{eLZO}O7g_e|hJ06i^g(1e zs}D1VE3;w@RAgo zHX4+a$2*u;{K=FMU5Wqz1${|GK~(stW!!}#l6O7YUxcH=#c%GWu3;_xaQC~!<8@R0 zj!;d%c;-9T&t-mQ)Wm2P!&!OpYC0DtyFLcH9@7_r6=!AIIe)tcC)tnp0jMoVJS7SG z4T_e;8&a-oWy{?-{d(|qaRA88jY{-4vnRQ6DoRRlo9~$p8QCrd9Rvo^?`dPst?Sp7Seoi3! z7!_VpdURKuu?B$yX9=MdU~NUdqYUvQ(9ZFfmx$3%pC|RV2TwDeM&EVt$Vx#&*DH)6 zlM8B^j`lj{fHg?;a%ecPI`q3ZNDv zc995lcT?@nghX}&Mujg$hw(66`xK}KA@#2Q4MVm0Z-2lN^8Uo;Yq2VUQge4fjQixz_m>CXDpwTqfEs-wCjDmK>yhs;J^@=E! z*uw?Q0>D;=O96Rwj4KOe>{?wgNsF;u!#B&dS71cR?sD0p5o3(__mmCOSpm1B;0=C2 zUzoOn5j@&c{kTk8wUUo)o)+gf(_ZhZm+SC~yRw)eXPPa)kl04aAO!b?C}M~^OGMjO z&mOfrN_~Qe^Sv3LP2fh)7;!p`WM|s#zyB+D=}6JNhnvWF(( z(_d<2&ydCSS(~P}ySvk{vH2p|HI@WpbSouaN?k@KLHAuYg$i#RU<1bDv*S&dBmg%9 zSV$TQ%}KcA(WH3ulcwNaI4IqZCX7npDUrLWn7QAdo8kA&tm?|Fup*#gPaTnoq7b+rI0iP5v?b$kRwmn)op zRK1j8N8Q3E?RZv8FIp*}y1`1ZbX7xr`r?$6m66s!@uP(?D!T@?Hi|S^G2*m~UW_WY zS>_`pOS%bAYXRWhmCL+_mc0saUa{XY8(+(wDb51m)c2?Mc#2O7OAxb688mH3tG^J8 z7DManz-00$sti!B^l$Il?7gPuQ8VC^+}xZ>dF2={f?0&hB3Y-fzF?_yUp(?n0qlq?cO6neSuA9t{a%z1u7qM8JzsR0Mf%uBjvLR0#Qp<(KJC8h3qypmetTao1a qG795>;-Mju3N{u{ni$v~bMp`W$d?tWJdUFP0000 + + + + + + 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:2592374569.2 %
Date:2024-12-17 22:35:29Functions:10712486.3 %
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 +
68.9%68.9%
+ 		68.9 %2549 / 370284.8 %95 / 112
<unnamed>68.9 %2549 / 370284.8 %95 / 112
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..cacd6fb677 --- /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:2592374569.2 %
Date:2024-12-17 22:35:29Functions:10712486.3 %
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 +
68.9%68.9%
+ 		68.9 %2549 / 370284.8 %95 / 112
<unnamed>68.9 %2549 / 370284.8 %95 / 112
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..ae4b2ac343 --- /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:2592374569.2 %
Date:2024-12-17 22:35:29Functions:10712486.3 %
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 +
68.9%68.9%
+ 		68.9 %2549 / 370284.8 %95 / 112
<unnamed>68.9 %2549 / 370284.8 %95 / 112
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..685a9b1f3d --- /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:2592374569.2 %
Date:2024-12-17 22:35:29Functions:10712486.3 %
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 +
68.9%68.9%
+ 		68.9 %2549 / 370284.8 %95 / 112
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..13c3b08f80 --- /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:2592374569.2 %
Date:2024-12-17 22:35:29Functions:10712486.3 %
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 +
68.9%68.9%
+ 		68.9 %2549 / 370284.8 %95 / 112
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..6149b5498a --- /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:2592374569.2 %
Date:2024-12-17 22:35:29Functions:10712486.3 %
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 +
68.9%68.9%
+ 		68.9 %2549 / 370284.8 %95 / 112
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..1a9d8dd325 --- /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-12-17 22:35:29Functions: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&)109
mrs_uav_managers::control_manager::OutputPublisher::OutputPublisher()109
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)508
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1086
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1137
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1439
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2474
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3976
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)4022
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)8249
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)103535
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&)126426
+
+
+ + + +
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..297008c0f3 --- /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-12-17 22:35:29Functions: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&)4022
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)8249
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)508
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1439
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)103535
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3976
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1086
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2474
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)1137
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&)126426
mrs_uav_managers::control_manager::OutputPublisher::OutputPublisher(ros::NodeHandle&)109
mrs_uav_managers::control_manager::OutputPublisher::OutputPublisher()109
+
+
+ + + +
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..520bc81e58 --- /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-12-17 22:35:29Functions: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         109 : OutputPublisher::OutputPublisher() {
+      10         109 : }
+      11             : 
+      12         109 : OutputPublisher::OutputPublisher(ros::NodeHandle& nh) {
+      13             : 
+      14         109 :   ph_hw_api_actuator_cmd_              = mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd>(nh, "hw_api_actuator_cmd_out", 1);
+      15         109 :   ph_hw_api_control_group_cmd_         = mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd>(nh, "hw_api_control_group_cmd_out", 1);
+      16         109 :   ph_hw_api_attitude_rate_cmd_         = mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd>(nh, "hw_api_attitude_rate_cmd_out", 1);
+      17         109 :   ph_hw_api_attitude_cmd_              = mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd>(nh, "hw_api_attitude_cmd_out", 1);
+      18         109 :   ph_hw_api_acceleration_hdg_rate_cmd_ = mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd>(nh, "hw_api_acceleration_hdg_rate_cmd_out", 1);
+      19         109 :   ph_hw_api_acceleration_hdg_cmd_      = mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd>(nh, "hw_api_acceleration_hdg_cmd_out", 1);
+      20         109 :   ph_hw_api_velocity_hdg_rate_cmd_     = mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd>(nh, "hw_api_velocity_hdg_rate_cmd_out", 1);
+      21         109 :   ph_hw_api_velocity_hdg_cmd_          = mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd>(nh, "hw_api_velocity_hdg_cmd_out", 1);
+      22         109 :   ph_hw_api_position_cmd_              = mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd>(nh, "hw_api_position_cmd_out", 1);
+      23         109 : }
+      24             : 
+      25      126426 : void OutputPublisher::publish(const Controller::HwApiOutputVariant& control_output) {
+      26             : 
+      27      126426 :   std::visit(OutputPublisher::PublisherVisitor(this), control_output);
+      28      126426 : }
+      29             : 
+      30             : // | ------------------------- private ------------------------ |
+      31             : 
+      32        4022 : void OutputPublisher::publish(const mrs_msgs::HwApiActuatorCmd& msg) {
+      33        4022 :   ph_hw_api_actuator_cmd_.publish(msg);
+      34        4022 : }
+      35             : 
+      36        3976 : void OutputPublisher::publish(const mrs_msgs::HwApiControlGroupCmd& msg) {
+      37        3976 :   ph_hw_api_control_group_cmd_.publish(msg);
+      38        3976 : }
+      39             : 
+      40      103535 : void OutputPublisher::publish(const mrs_msgs::HwApiAttitudeRateCmd& msg) {
+      41      103535 :   ph_hw_api_attitude_rate_cmd_.publish(msg);
+      42      103535 : }
+      43             : 
+      44        8249 : void OutputPublisher::publish(const mrs_msgs::HwApiAttitudeCmd& msg) {
+      45        8249 :   ph_hw_api_attitude_cmd_.publish(msg);
+      46        8249 : }
+      47             : 
+      48        1137 : void OutputPublisher::publish(const mrs_msgs::HwApiAccelerationHdgRateCmd& msg) {
+      49        1137 :   ph_hw_api_acceleration_hdg_rate_cmd_.publish(msg);
+      50        1137 : }
+      51             : 
+      52        1086 : void OutputPublisher::publish(const mrs_msgs::HwApiAccelerationHdgCmd& msg) {
+      53        1086 :   ph_hw_api_acceleration_hdg_cmd_.publish(msg);
+      54        1086 : }
+      55             : 
+      56        2474 : void OutputPublisher::publish(const mrs_msgs::HwApiVelocityHdgRateCmd& msg) {
+      57        2474 :   ph_hw_api_velocity_hdg_rate_cmd_.publish(msg);
+      58        2474 : }
+      59             : 
+      60        1439 : void OutputPublisher::publish(const mrs_msgs::HwApiVelocityHdgCmd& msg) {
+      61        1439 :   ph_hw_api_velocity_hdg_cmd_.publish(msg);
+      62        1439 : }
+      63             : 
+      64         508 : void OutputPublisher::publish(const mrs_msgs::HwApiPositionCmd& msg) {
+      65         508 :   ph_hw_api_position_cmd_.publish(msg);
+      66         508 : }
+      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..3064b6385b --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.func-sort-c.html @@ -0,0 +1,180 @@ + + + + + + + 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:37557864.9 %
Date:2024-12-17 22:35:29Functions:212584.0 %
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::callbackResetEstimator(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)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()109
mrs_uav_managers::estimation_manager::StateMachine::StateMachine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)109
mrs_uav_managers::estimation_manager::EstimationManager::timerInitialization(ros::TimerEvent const&)109
mrs_uav_managers::estimation_manager::EstimationManager::onInit()109
mrs_uav_managers::estimation_manager::EstimationManager::EstimationManager()109
mrs_uav_managers::estimation_manager::EstimationManager::callbackToggleServiceCallbacks(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)144
mrs_uav_managers::estimation_manager::StateMachine::changeState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&)350
mrs_uav_managers::estimation_manager::StateMachine::getPrintName[abi:cxx11]() const350
mrs_uav_managers::estimation_manager::StateMachine::getStateAsString[abi:cxx11](mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const700
mrs_uav_managers::estimation_manager::EstimationManager::getName[abi:cxx11]() const3631
mrs_uav_managers::estimation_manager::StateMachine::getCurrentStateString[abi:cxx11]() const20010
mrs_uav_managers::estimation_manager::EstimationManager::timerPublishDiagnostics(ros::TimerEvent const&)21272
mrs_uav_managers::estimation_manager::EstimationManager::timerPublish(ros::TimerEvent const&)198314
mrs_uav_managers::estimation_manager::StateMachine::isInSwitchableState() const198315
mrs_uav_managers::estimation_manager::EstimationManager::timerCheckHealth(ros::TimerEvent const&)198316
mrs_uav_managers::estimation_manager::StateMachine::isInPublishableState() const219558
mrs_uav_managers::estimation_manager::StateMachine::isInitialized() const417774
mrs_uav_managers::estimation_manager::StateMachine::isInState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const1908000
+
+
+ + + +
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..24c9417976 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.func.html @@ -0,0 +1,180 @@ + + + + + + + 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:37557864.9 %
Date:2024-12-17 22:35:29Functions:212584.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()109
mrs_uav_managers::estimation_manager::StateMachine::changeState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&)350
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&)109
mrs_uav_managers::estimation_manager::EstimationManager::timerPublish(ros::TimerEvent const&)198314
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&)198316
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&)109
mrs_uav_managers::estimation_manager::EstimationManager::callbackResetEstimator(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)0
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&)21272
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> >&)144
mrs_uav_managers::estimation_manager::EstimationManager::onInit()109
mrs_uav_managers::estimation_manager::EstimationManager::EstimationManager()109
mrs_uav_managers::estimation_manager::StateMachine::getPrintName[abi:cxx11]() const350
mrs_uav_managers::estimation_manager::StateMachine::isInitialized() const417774
mrs_uav_managers::estimation_manager::StateMachine::getStateAsString[abi:cxx11](mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const700
mrs_uav_managers::estimation_manager::StateMachine::isInSwitchableState() const198315
mrs_uav_managers::estimation_manager::StateMachine::isInPublishableState() const219558
mrs_uav_managers::estimation_manager::StateMachine::getCurrentStateString[abi:cxx11]() const20010
mrs_uav_managers::estimation_manager::StateMachine::isInState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const1908000
mrs_uav_managers::estimation_manager::EstimationManager::getName[abi:cxx11]() const3631
+
+
+ + + +
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..db27180769 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.html @@ -0,0 +1,1412 @@ + + + + + + + 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:37557864.9 %
Date:2024-12-17 22:35:29Functions:212584.0 %
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        1526 :   StateMachine(const std::string& nodelet_name) : nodelet_name_(nodelet_name) {
+      77         109 :   }
+      78             : 
+      79     1908000 :   bool isInState(const SMState_t& state) const {
+      80     1908000 :     return mrs_lib::get_mutexed(mtx_state_, current_state_) == state;
+      81             :   }
+      82             : 
+      83      417774 :   bool isInitialized() const {
+      84      417774 :     return mrs_lib::get_mutexed(mtx_state_, current_state_) != UNINITIALIZED_STATE;
+      85             :   }
+      86             : 
+      87      219558 :   bool isInPublishableState() const {
+      88      219558 :     const SMState_t current_state = mrs_lib::get_mutexed(mtx_state_, current_state_);
+      89      169659 :     return current_state == READY_FOR_FLIGHT_STATE || current_state == TAKING_OFF_STATE || current_state == HOVER_STATE || current_state == FLYING_STATE ||
+      90      389244 :            current_state == LANDING_STATE || current_state == DUMMY_STATE || current_state == FAILSAFE_STATE;
+      91             :   }
+      92             : 
+      93      198315 :   bool isInSwitchableState() const {
+      94      198315 :     const SMState_t current_state = mrs_lib::get_mutexed(mtx_state_, current_state_);
+      95      198315 :     return current_state == READY_FOR_FLIGHT_STATE || current_state == TAKING_OFF_STATE || current_state == HOVER_STATE || current_state == FLYING_STATE ||
+      96      198315 :            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       20010 :   std::string getCurrentStateString() const {
+     109       20010 :     return mrs_lib::get_mutexed(mtx_state_, sm_state_names_[current_state_]);
+     110             :   }
+     111             : 
+     112         700 :   std::string getStateAsString(const SMState_t& state) const {
+     113         700 :     return sm_state_names_[state];
+     114             :   }
+     115             : 
+     116             :   /*//{ changeState() */
+     117         350 :   bool changeState(const SMState_t& target_state) {
+     118             : 
+     119         350 :     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         350 :     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         109 :       case INITIALIZED_STATE: {
+     135         109 :         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         109 :         break;
+     141             :       }
+     142             : 
+     143         109 :       case READY_FOR_FLIGHT_STATE: {
+     144         109 :         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         109 :         break;
+     151             :       }
+     152             : 
+     153          23 :       case TAKING_OFF_STATE: {
+     154          23 :         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          23 :         break;
+     160             :       }
+     161             : 
+     162         104 :       case FLYING_STATE: {
+     163         104 :         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         104 :         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         350 :     std::scoped_lock lock(mtx_state_);
+     241             :     {
+     242         350 :       previous_state_ = current_state_;
+     243         350 :       current_state_  = target_state;
+     244             :     }
+     245             : 
+     246         350 :     ROS_INFO("[%s]: successfully changed states %s -> %s", getPrintName().c_str(), getStateAsString(previous_state_).c_str(),
+     247             :              getStateAsString(current_state_).c_str());
+     248             : 
+     249         350 :     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         350 :   std::string getPrintName() const {
+     274         700 :     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             :   ros::ServiceServer srvs_reset_estimator_;
+     349             :   bool               callbackResetEstimator(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+     350             : 
+     351             : 
+     352             :   ros::ServiceServer srvs_toggle_callbacks_;
+     353             :   bool               callbackToggleServiceCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     354             :   bool               callbacks_enabled_             = false;
+     355             :   bool               callbacks_disabled_by_service_ = false;
+     356             : 
+     357             :   bool                                             callFailsafeService();
+     358             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> srvch_failsafe_;
+     359             :   bool                                             failsafe_call_succeeded_ = false;
+     360             : 
+     361             :   // TODO service clients
+     362             :   /* mrs_lib::ServiceClientHandler<std_srvs::Trigger> srvc_hover_; */
+     363             :   /* mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv> srvc_reference_; */
+     364             :   /* mrs_lib::ServiceClientHandler<std_srvs::Trigger> srvc_ehover_; */
+     365             :   /* mrs_lib::ServiceClientHandler<std_srvs::SetBool> srvc_enable_callbacks_; */
+     366             : 
+     367             :   // | ------------- dynamic loading of estimators ------------- |
+     368             :   std::unique_ptr<pluginlib::ClassLoader<mrs_uav_managers::StateEstimator>> state_estimator_loader_;  // pluginlib loader of dynamically loaded estimators
+     369             :   std::vector<std::string>                                                  estimator_names_;         // list of estimator names
+     370             :   std::vector<boost::shared_ptr<mrs_uav_managers::StateEstimator>>          estimator_list_;          // list of estimators
+     371             :   std::mutex                                                                mutex_estimator_list_;
+     372             :   std::vector<std::string>                                                  switchable_estimator_names_;
+     373             :   std::mutex                                                                mutex_switchable_estimator_names_;
+     374             :   std::string                                                               initial_estimator_name_ = "UNDEFINED_INITIAL_ESTIMATOR";
+     375             :   boost::shared_ptr<mrs_uav_managers::StateEstimator>                       initial_estimator_;
+     376             :   boost::shared_ptr<mrs_uav_managers::StateEstimator>                       active_estimator_;
+     377             :   std::mutex                                                                mutex_active_estimator_;
+     378             : 
+     379             :   std::unique_ptr<pluginlib::ClassLoader<mrs_uav_managers::AglEstimator>> agl_estimator_loader_;  // pluginlib loader of dynamically loaded estimators
+     380             :   std::string                                                             est_alt_agl_name_ = "UNDEFINED_AGL_ESTIMATOR";
+     381             :   boost::shared_ptr<mrs_uav_managers::AglEstimator>                       est_alt_agl_;
+     382             :   bool                                                                    is_using_agl_estimator_;
+     383             : 
+     384             :   double max_flight_z_;
+     385             : 
+     386             :   bool switchToHealthyEstimator();
+     387             :   void switchToEstimator(const boost::shared_ptr<mrs_uav_managers::StateEstimator>& target_estimator);
+     388             : 
+     389             :   bool loadConfigFile(const std::string& file_path);
+     390             : 
+     391             : public:
+     392         109 :   EstimationManager() {
+     393         109 :   }
+     394             : 
+     395             :   void onInit();
+     396             : 
+     397             :   std::string getName() const;
+     398             : };
+     399             : /*//}*/
+     400             : 
+     401             : /*//{ onInit() */
+     402         109 : void EstimationManager::onInit() {
+     403             : 
+     404         109 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     405             : 
+     406         109 :   ros::Time::waitForValid();
+     407             : 
+     408         109 :   ROS_INFO("[%s]: initializing", getName().c_str());
+     409             : 
+     410         109 :   sm_ = std::make_shared<StateMachine>(nodelet_name_);
+     411             : 
+     412         109 :   ch_ = std::make_shared<CommonHandlers_t>();
+     413             : 
+     414         109 :   ch_->nodelet_name = nodelet_name_;
+     415         109 :   ch_->package_name = package_name_;
+     416             : 
+     417             :   // finish initialization in a oneshot timer, so that we don't block loading of other nodelets by the nodelet manager
+     418         109 :   timer_initialization_ = nh_.createTimer(ros::Rate(1.0), &EstimationManager::timerInitialization, this, true, true);
+     419         109 : }
+     420             : /*//}*/
+     421             : 
+     422             : // --------------------------------------------------------------
+     423             : // |                          callbacks                         |
+     424             : // --------------------------------------------------------------
+     425             : 
+     426             : // | --------------------- timer callbacks -------------------- |
+     427             : 
+     428             : /*//{ timerPublish() */
+     429      198314 : void EstimationManager::timerPublish([[maybe_unused]] const ros::TimerEvent& event) {
+     430             : 
+     431      198314 :   if (!sm_->isInitialized()) {
+     432       22037 :     return;
+     433             :   }
+     434             : 
+     435      396626 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("EstimationManager::timerPublish", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     436             : 
+     437      198313 :   if (sm_->isInState(StateMachine::ESTIMATOR_SWITCHING_STATE)) {
+     438           0 :     ROS_WARN("[%s]: Not publishing during estimator switching.", getName().c_str());
+     439           0 :     return;
+     440             :   }
+     441             : 
+     442      198313 :   if (!sm_->isInPublishableState()) {
+     443       22036 :     ROS_WARN_THROTTLE(1.0, "[%s]: not publishing uav state in %s", getName().c_str(), sm_->getCurrentStateString().c_str());
+     444       22036 :     return;
+     445             :   }
+     446             : 
+     447      176277 :   mrs_msgs::UavState uav_state;
+     448      176277 :   auto               ret = active_estimator_->getUavState();
+     449      176277 :   if (ret) {
+     450      176277 :     uav_state = ret.value();
+     451             :   } else {
+     452           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: Active estimator did not provide uav_state.", getName().c_str());
+     453           0 :     return;
+     454             :   }
+     455             : 
+     456      176277 :   if (!Support::noNans(uav_state.pose.orientation)) {
+     457           0 :     ROS_ERROR("[%s]: nan in uav state orientation", getName().c_str());
+     458           0 :     return;
+     459             :   }
+     460             : 
+     461             : 
+     462      176277 :   if (active_estimator_->isMitigatingJump()) {
+     463           0 :     estimator_switch_count_++;
+     464             :   }
+     465      176277 :   uav_state.estimator_iteration = estimator_switch_count_;
+     466             : 
+     467      176277 :   scope_timer.checkpoint("get uav state");
+     468             :   // TODO state health checks
+     469             : 
+     470      176277 :   ph_uav_state_.publish(uav_state);
+     471             : 
+     472      176277 :   scope_timer.checkpoint("pub uav state");
+     473      352554 :   nav_msgs::Odometry odom_main = Support::uavStateToOdom(uav_state);
+     474             : 
+     475      176277 :   scope_timer.checkpoint("uav state to odom");
+     476      352554 :   const std::vector<double> pose_covariance = active_estimator_->getPoseCovariance();
+     477     6522247 :   for (size_t i = 0; i < pose_covariance.size(); i++) {
+     478     6345966 :     odom_main.pose.covariance[i] = pose_covariance[i];
+     479             :   }
+     480             : 
+     481      352554 :   const std::vector<double> twist_covariance = active_estimator_->getTwistCovariance();
+     482     6522247 :   for (size_t i = 0; i < twist_covariance.size(); i++) {
+     483     6345966 :     odom_main.twist.covariance[i] = twist_covariance[i];
+     484             :   }
+     485             : 
+     486      176277 :   scope_timer.checkpoint("get covariance");
+     487      176277 :   ph_odom_main_.publish(odom_main);
+     488             : 
+     489      352554 :   nav_msgs::Odometry innovation = active_estimator_->getInnovation();
+     490      176277 :   ph_innovation_.publish(innovation);
+     491             : 
+     492             : 
+     493      352554 :   mrs_msgs::Float64Stamped agl_height;
+     494             : 
+     495      176277 :   if (is_using_agl_estimator_) {
+     496      119514 :     agl_height = est_alt_agl_->getUavAglHeight();
+     497      119514 :     ph_altitude_agl_.publish(agl_height);
+     498             :   }
+     499             : }
+     500             : /*//}*/
+     501             : 
+     502             : /*//{ timerPublishDiagnostics() */
+     503       21272 : void EstimationManager::timerPublishDiagnostics([[maybe_unused]] const ros::TimerEvent& event) {
+     504             : 
+     505       21272 :   if (!sm_->isInitialized()) {
+     506        2184 :     return;
+     507             :   }
+     508             : 
+     509       42544 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("EstimationManager::timerPublishDiagnostics", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     510             : 
+     511       21272 :   if (sm_->isInState(StateMachine::ESTIMATOR_SWITCHING_STATE)) {
+     512           0 :     ROS_WARN("[%s]: Not publishing diagnostics during estimator switching.", getName().c_str());
+     513           0 :     return;
+     514             :   }
+     515             : 
+     516       21272 :   if (!sm_->isInPublishableState()) {
+     517        2184 :     ROS_WARN_THROTTLE(1.0, "[%s]: not publishing uav state in %s", getName().c_str(), sm_->getCurrentStateString().c_str());
+     518        2184 :     return;
+     519             :   }
+     520             : 
+     521       19088 :   mrs_msgs::UavState uav_state;
+     522       19088 :   auto               ret = active_estimator_->getUavState();
+     523       19088 :   if (ret) {
+     524       19088 :     uav_state = ret.value();
+     525             :   } else {
+     526           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: Active estimator did not provide uav_state.", getName().c_str());
+     527           0 :     return;
+     528             :   }
+     529             : 
+     530       19088 :   if (!Support::noNans(uav_state.pose.orientation)) {
+     531           0 :     ROS_ERROR("[%s]: nan in uav state orientation", getName().c_str());
+     532           0 :     return;
+     533             :   }
+     534             : 
+     535       38176 :   mrs_msgs::Float64Stamped agl_height;
+     536             : 
+     537       19088 :   if (is_using_agl_estimator_) {
+     538       12164 :     agl_height = est_alt_agl_->getUavAglHeight();
+     539       12164 :     ph_altitude_agl_.publish(agl_height);
+     540             :   }
+     541             : 
+     542       38176 :   mrs_msgs::Float64Stamped max_flight_z_msg;
+     543       19088 :   max_flight_z_msg.header.stamp = ros::Time::now();
+     544       19088 :   if (active_estimator_ && active_estimator_->isInitialized()) {
+     545       19088 :     max_flight_z_msg.header.frame_id = active_estimator_->getFrameId();
+     546       19088 :     max_flight_z_msg.value           = active_estimator_->getMaxFlightZ();
+     547             :   }
+     548       19088 :   max_flight_z_ = max_flight_z_msg.value;
+     549       19088 :   ph_max_flight_z_.publish(max_flight_z_msg);
+     550             : 
+     551             :   // publish diagnostics
+     552       38176 :   mrs_msgs::EstimationDiagnostics diagnostics;
+     553             : 
+     554       19088 :   diagnostics.header.stamp   = uav_state.header.stamp;
+     555       19088 :   diagnostics.child_frame_id = uav_state.child_frame_id;
+     556             : 
+     557       19088 :   diagnostics.pose         = uav_state.pose;
+     558       19088 :   diagnostics.velocity     = uav_state.velocity;
+     559       19088 :   diagnostics.acceleration = uav_state.acceleration;
+     560             : 
+     561       19088 :   diagnostics.sm_state                 = sm_->getCurrentStateString();
+     562       19088 :   diagnostics.max_flight_z             = max_flight_z_msg.value;
+     563       19088 :   diagnostics.estimator_iteration      = estimator_switch_count_;
+     564       19088 :   diagnostics.estimation_rate          = ch_->desired_uav_state_rate;
+     565       19088 :   diagnostics.running_state_estimators = estimator_names_;
+     566             : 
+     567             :   {
+     568       38176 :     std::scoped_lock lock(mutex_switchable_estimator_names_);
+     569       19088 :     diagnostics.switchable_state_estimators = switchable_estimator_names_;
+     570             :   }
+     571             : 
+     572             : 
+     573       19088 :   if (active_estimator_ && active_estimator_->isInitialized()) {
+     574       19088 :     diagnostics.header.frame_id         = active_estimator_->getFrameId();
+     575       19088 :     diagnostics.current_state_estimator = active_estimator_->getName();
+     576             :   } else {
+     577           0 :     diagnostics.header.frame_id         = "";
+     578           0 :     diagnostics.current_state_estimator = "";
+     579             :   }
+     580             : 
+     581       19088 :   diagnostics.estimator_horizontal = uav_state.estimator_horizontal;
+     582       19088 :   diagnostics.estimator_vertical   = uav_state.estimator_vertical;
+     583       19088 :   diagnostics.estimator_heading    = uav_state.estimator_heading;
+     584             : 
+     585       19088 :   if (is_using_agl_estimator_) {
+     586       12164 :     diagnostics.estimator_agl_height = est_alt_agl_name_;
+     587       12164 :     diagnostics.agl_height           = agl_height.value;
+     588             :   } else {
+     589        6924 :     diagnostics.estimator_agl_height = "NOT_USED";
+     590        6924 :     diagnostics.agl_height           = std::nanf("");
+     591             :   }
+     592             : 
+     593       19088 :   diagnostics.platform_config = _platform_config_;
+     594       19088 :   diagnostics.custom_config   = _custom_config_;
+     595             : 
+     596       19088 :   ph_diagnostics_.publish(diagnostics);
+     597             : 
+     598       19088 :   ROS_INFO_THROTTLE(5.0, "[%s]: %s. pos: [%.2f, %.2f, %.2f] m. Estimator: %s. Max. z.: %.2f m. Estimator switches: %d.", getName().c_str(),
+     599             :                     sm_->getCurrentStateString().c_str(), uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z,
+     600             :                     active_estimator_->getName().c_str(), max_flight_z_, estimator_switch_count_);
+     601             : }
+     602             : /*//}*/
+     603             : 
+     604             : /*//{ timerCheckHealth() */
+     605      198316 : void EstimationManager::timerCheckHealth([[maybe_unused]] const ros::TimerEvent& event) {
+     606             : 
+     607      198316 :   if (!sm_->isInitialized()) {
+     608           1 :     return;
+     609             :   }
+     610             : 
+     611      594945 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("EstimationManager::timerCheckHealth", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     612             : 
+     613             :   /*//{ start ready estimators, check switchable estimators */
+     614      396630 :   std::vector<std::string> switchable_estimator_names;
+     615      421103 :   for (auto estimator : estimator_list_) {
+     616             : 
+     617      222788 :     if (estimator->isReady()) {
+     618             :       try {
+     619        8317 :         ROS_INFO_THROTTLE(1.0, "[%s]: starting the estimator '%s'", getName().c_str(), estimator->getName().c_str());
+     620        8317 :         estimator->start();
+     621             :       }
+     622           0 :       catch (std::runtime_error& ex) {
+     623           0 :         ROS_ERROR("[%s]: exception caught during estimator starting: '%s'", getName().c_str(), ex.what());
+     624           0 :         ros::shutdown();
+     625             :       }
+     626             :     }
+     627             : 
+     628      222788 :     if (estimator->isRunning() && estimator->getName() != "dummy" && estimator->getName() != "ground_truth") {
+     629      211212 :       switchable_estimator_names.push_back(estimator->getName());
+     630             :     }
+     631             :   }
+     632             : 
+     633             :   {
+     634      396630 :     std::scoped_lock lock(mutex_switchable_estimator_names_);
+     635      198315 :     switchable_estimator_names_ = switchable_estimator_names;
+     636             :   }
+     637             : 
+     638      198315 :   if (is_using_agl_estimator_ && est_alt_agl_->isReady()) {
+     639         187 :     est_alt_agl_->start();
+     640             :   }
+     641             : 
+     642             :   /*//}*/
+     643             : 
+     644      363983 :   if (!callbacks_disabled_by_service_ &&
+     645      363983 :       (sm_->isInState(StateMachine::FLYING_STATE) || sm_->isInState(StateMachine::HOVER_STATE) || sm_->isInState(StateMachine::READY_FOR_FLIGHT_STATE))) {
+     646      143194 :     callbacks_enabled_ = true;
+     647             :   } else {
+     648       55121 :     callbacks_enabled_ = false;
+     649             :   }
+     650             : 
+     651             :   // TODO fuj if, zmenit na switch
+     652             :   // activate initial estimator
+     653      198315 :   if (sm_->isInState(StateMachine::INITIALIZED_STATE) && initial_estimator_->isRunning()) {
+     654       21292 :     std::scoped_lock lock(mutex_active_estimator_);
+     655       10646 :     ROS_INFO_THROTTLE(1.0, "[%s]: activating the initial estimator %s", getName().c_str(), initial_estimator_->getName().c_str());
+     656       10646 :     active_estimator_ = initial_estimator_;
+     657       10646 :     if (active_estimator_->getName() == "dummy") {
+     658           0 :       sm_->changeState(StateMachine::DUMMY_STATE);
+     659             :     } else {
+     660       10646 :       if (!is_using_agl_estimator_ || est_alt_agl_->isRunning()) {
+     661         109 :         sm_->changeState(StateMachine::READY_FOR_FLIGHT_STATE);
+     662             :       } else {
+     663       10537 :         ROS_INFO_THROTTLE(1.0, "[%s]: %s agl estimator: %s to be running", getName().c_str(), Support::waiting_for_string.c_str(),
+     664             :                           est_alt_agl_->getName().c_str());
+     665             :       }
+     666             :     }
+     667             :   }
+     668             : 
+     669             :   // active estimator is in faulty state, we need to switch to healthy estimator
+     670      198315 :   if (sm_->isInSwitchableState() && active_estimator_->isError()) {
+     671           0 :     sm_->changeState(StateMachine::ESTIMATOR_SWITCHING_STATE);
+     672             :   }
+     673             : 
+     674      198315 :   if (sm_->isInState(StateMachine::ESTIMATOR_SWITCHING_STATE)) {
+     675           0 :     if (switchToHealthyEstimator()) {
+     676           0 :       sm_->changeToPreSwitchState();
+     677             :     } else {  // cannot switch to healthy estimator - failsafe necessary
+     678           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: Cannot switch to any healthy estimator. Triggering failsafe.", getName().c_str());
+     679           0 :       sm_->changeState(StateMachine::FAILSAFE_STATE);
+     680             :     }
+     681             :   }
+     682             : 
+     683      198315 :   if (sm_->isInState(StateMachine::FAILSAFE_STATE)) {
+     684           0 :     if (!failsafe_call_succeeded_ && callFailsafeService()) {
+     685           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: failsafe called successfully", getName().c_str());
+     686           0 :       failsafe_call_succeeded_ = true;
+     687             :     }
+     688           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: we are in failsafe state", getName().c_str());
+     689             :   }
+     690             : 
+     691             :   // standard takeoff
+     692      198315 :   if (sm_->isInState(StateMachine::READY_FOR_FLIGHT_STATE)) {
+     693       45343 :     if (sh_control_manager_diag_.hasMsg() && sh_control_manager_diag_.getMsg()->active_tracker == takeoff_tracker_name_) {
+     694          23 :       sm_->changeState(StateMachine::TAKING_OFF_STATE);
+     695             :     }
+     696             :   }
+     697             : 
+     698             :   // midair activation
+     699      198315 :   if (sm_->isInState(StateMachine::READY_FOR_FLIGHT_STATE)) {
+     700       45320 :     if (sh_control_manager_diag_.hasMsg() && sh_control_manager_diag_.getMsg()->active_tracker == after_midair_activation_tracker_name_) {
+     701          78 :       sm_->changeState(StateMachine::FLYING_STATE);
+     702             :     }
+     703             :   }
+     704             : 
+     705      198315 :   if (sm_->isInState(StateMachine::TAKING_OFF_STATE)) {
+     706       11330 :     if (sh_control_manager_diag_.hasMsg() && sh_control_manager_diag_.getMsg()->active_tracker != takeoff_tracker_name_) {
+     707          21 :       sm_->changeState(StateMachine::FLYING_STATE);
+     708             :     }
+     709             :   }
+     710             : 
+     711      198315 :   if (sm_->isInState(StateMachine::FLYING_STATE)) {
+     712      119815 :     if (!sh_control_input_.hasMsg()) {
+     713        4341 :       ROS_WARN_THROTTLE(1.0, "[%s]: not received control input since starting EstimationManager, estimation suboptimal, potentially unstable", getName().c_str());
+     714      115474 :     } else if ((ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {
+     715       13158 :       ROS_WARN_THROTTLE(1.0, "[%s]: not received control input for %.4fs, estimation suboptimal, potentially unstable", getName().c_str(),
+     716             :                         (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec());
+     717             :     }
+     718             :   }
+     719             : }
+     720             : /*//}*/
+     721             : 
+     722             : /*//{ timerInitialization() */
+     723         109 : void EstimationManager::timerInitialization([[maybe_unused]] const ros::TimerEvent& event) {
+     724             : 
+     725         218 :   mrs_lib::ParamLoader param_loader(nh_, getName());
+     726             : 
+     727         109 :   param_loader.loadParam("custom_config", _custom_config_);
+     728         109 :   param_loader.loadParam("platform_config", _platform_config_);
+     729         109 :   param_loader.loadParam("world_config", _world_config_);
+     730             : 
+     731         109 :   if (_custom_config_ != "") {
+     732         109 :     param_loader.addYamlFile(_custom_config_);
+     733             :   }
+     734             : 
+     735         109 :   if (_platform_config_ != "") {
+     736         109 :     param_loader.addYamlFile(_platform_config_);
+     737             :   }
+     738             : 
+     739         109 :   if (_world_config_ != "") {
+     740         109 :     param_loader.addYamlFile(_world_config_);
+     741             :   }
+     742             : 
+     743         109 :   param_loader.addYamlFileFromParam("private_config");
+     744         109 :   param_loader.addYamlFileFromParam("public_config");
+     745         109 :   param_loader.addYamlFileFromParam("uav_manager_config");
+     746         109 :   param_loader.addYamlFileFromParam("estimators_config");
+     747         109 :   param_loader.addYamlFileFromParam("active_estimators_config");
+     748             : 
+     749         109 :   param_loader.loadParam("uav_name", ch_->uav_name);
+     750             : 
+     751             :   /*//{ load world_origin parameters */
+     752             : 
+     753         218 :   std::string world_origin_units;
+     754         109 :   bool        is_origin_param_ok = true;
+     755         109 :   double      world_origin_x     = 0;
+     756         109 :   double      world_origin_y     = 0;
+     757             : 
+     758         109 :   param_loader.loadParam("world_origin/units", world_origin_units);
+     759             : 
+     760         109 :   if (Support::toLowercase(world_origin_units) == "utm") {
+     761           0 :     ROS_INFO("[%s]: Loading world origin in UTM units.", getName().c_str());
+     762           0 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_x", world_origin_x);
+     763           0 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_y", world_origin_y);
+     764             : 
+     765         109 :   } else if (Support::toLowercase(world_origin_units) == "latlon") {
+     766             :     double lat, lon;
+     767         109 :     ROS_INFO("[%s]: Loading world origin in LatLon units.", getName().c_str());
+     768         109 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_x", lat);
+     769         109 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_y", lon);
+     770         109 :     mrs_lib::UTM(lat, lon, &world_origin_x, &world_origin_y);
+     771         109 :     ROS_INFO("[%s]: Converted to UTM x: %f, y: %f.", getName().c_str(), world_origin_x, world_origin_y);
+     772             : 
+     773             :   } else {
+     774           0 :     ROS_ERROR("[%s]: world_origin_units must be (\"UTM\"|\"LATLON\"). Got '%s'", getName().c_str(), world_origin_units.c_str());
+     775           0 :     ros::shutdown();
+     776             :   }
+     777             : 
+     778         109 :   ch_->world_origin.x = world_origin_x;
+     779         109 :   ch_->world_origin.y = world_origin_y;
+     780             : 
+     781         109 :   if (!is_origin_param_ok) {
+     782           0 :     ROS_ERROR("[%s]: Could not load all mandatory parameters from world file. Please check your world file.", getName().c_str());
+     783           0 :     ros::shutdown();
+     784             :   }
+     785             :   /*//}*/
+     786             : 
+     787             :   /*//{ load tracker names */
+     788         109 :   param_loader.loadParam("mrs_uav_managers/uav_manager/takeoff/during_takeoff/tracker", takeoff_tracker_name_);
+     789         109 :   param_loader.loadParam("mrs_uav_managers/uav_manager/midair_activation/after_activation/tracker", after_midair_activation_tracker_name_);
+     790             :   /*//}*/
+     791             : 
+     792         109 :   param_loader.setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/");
+     793             : 
+     794             :   /*//{ load parameters into common handlers */
+     795         109 :   param_loader.loadParam("frame_id/fcu", ch_->frames.fcu);
+     796         109 :   ch_->frames.ns_fcu = ch_->uav_name + "/" + ch_->frames.fcu;
+     797             : 
+     798         109 :   param_loader.loadParam("frame_id/fcu_untilted", ch_->frames.fcu_untilted);
+     799         109 :   ch_->frames.ns_fcu_untilted = ch_->uav_name + "/" + ch_->frames.fcu_untilted;
+     800             : 
+     801         109 :   param_loader.loadParam("frame_id/rtk_antenna", ch_->frames.rtk_antenna);
+     802         109 :   ch_->frames.ns_rtk_antenna = ch_->uav_name + "/" + ch_->frames.rtk_antenna;
+     803             : 
+     804         109 :   ch_->transformer = std::make_shared<mrs_lib::Transformer>(nh_, getName());
+     805         109 :   ch_->transformer->retryLookupNewest(true);
+     806             : 
+     807         109 :   param_loader.loadParam("rate/diagnostics", ch_->desired_diagnostics_rate);
+     808             : 
+     809             :   /*//}*/
+     810             : 
+     811             :   /*//{ load parameters */
+     812             : 
+     813             :   /*//{ publish debug topics parameters */
+     814         109 :   param_loader.loadParam("debug_topics/input", ch_->debug_topics.input);
+     815         109 :   param_loader.loadParam("debug_topics/output", ch_->debug_topics.output);
+     816         109 :   param_loader.loadParam("debug_topics/state", ch_->debug_topics.state);
+     817         109 :   param_loader.loadParam("debug_topics/covariance", ch_->debug_topics.covariance);
+     818         109 :   param_loader.loadParam("debug_topics/innovation", ch_->debug_topics.innovation);
+     819         109 :   param_loader.loadParam("debug_topics/diagnostics", ch_->debug_topics.diag);
+     820         109 :   param_loader.loadParam("debug_topics/correction", ch_->debug_topics.correction);
+     821         109 :   param_loader.loadParam("debug_topics/correction_delay", ch_->debug_topics.corr_delay);
+     822             :   /*//}*/
+     823             : 
+     824             :   /*//}*/
+     825             : 
+     826         218 :   mrs_lib::SubscribeHandlerOptions shopts;
+     827         109 :   shopts.nh                 = nh_;
+     828         109 :   shopts.node_name          = getName();
+     829         109 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     830         109 :   shopts.threadsafe         = true;
+     831         109 :   shopts.autostart          = true;
+     832         109 :   shopts.queue_size         = 10;
+     833         109 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     834             : 
+     835             :   /*//{ wait for hw api message */
+     836             : 
+     837             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities> sh_hw_api_capabilities_ =
+     838         327 :       mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities>(shopts, "hw_api_capabilities_in");
+     839         220 :   while (!sh_hw_api_capabilities_.hasMsg()) {
+     840         111 :     ROS_INFO("[%s]: %s hw_api_capabilities message at topic: %s", getName().c_str(), Support::waiting_for_string.c_str(),
+     841             :              sh_hw_api_capabilities_.topicName().c_str());
+     842         111 :     ros::Duration(1.0).sleep();
+     843             :   }
+     844             : 
+     845         218 :   mrs_msgs::HwApiCapabilitiesConstPtr hw_api_capabilities = sh_hw_api_capabilities_.getMsg();
+     846             :   /*//}*/
+     847             : 
+     848             :   /*//{ wait for desired uav_state rate */
+     849         109 :   sh_control_manager_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>(shopts, "control_manager_diagnostics_in");
+     850         231 :   while (!sh_control_manager_diag_.hasMsg()) {
+     851         122 :     ROS_INFO("[%s]: %s control_manager_diagnostics message at topic: %s", getName().c_str(), Support::waiting_for_string.c_str(),
+     852             :              sh_control_manager_diag_.topicName().c_str());
+     853         122 :     ros::Duration(1.0).sleep();
+     854             :   }
+     855             : 
+     856         218 :   mrs_msgs::ControlManagerDiagnosticsConstPtr control_manager_diag_msg = sh_control_manager_diag_.getMsg();
+     857         109 :   ch_->desired_uav_state_rate                                          = control_manager_diag_msg->desired_uav_state_rate;
+     858         109 :   ROS_INFO("[%s]: The estimation is running at: %.2f Hz", getName().c_str(), ch_->desired_uav_state_rate);
+     859             :   /*//}*/
+     860             : 
+     861             :   /*//{ initialize subscribers */
+     862             : 
+     863         109 :   sh_control_input_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput>(shopts, "control_input_in");
+     864             : 
+     865             :   /*//}*/
+     866             : 
+     867             :   /*//{ load state estimator plugins */
+     868         109 :   param_loader.loadParam("state_estimators", estimator_names_);
+     869             : 
+     870         109 :   state_estimator_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::StateEstimator>>("mrs_uav_managers", "mrs_uav_managers::StateEstimator");
+     871             : 
+     872         226 :   for (size_t i = 0; i < estimator_names_.size(); i++) {
+     873             : 
+     874         234 :     const std::string estimator_name = estimator_names_[i];
+     875             : 
+     876             :     // load the estimator parameters
+     877         234 :     std::string address;
+     878         117 :     param_loader.loadParam(estimator_name + "/address", address);
+     879             : 
+     880             :     try {
+     881         117 :       ROS_INFO("[%s]: loading the estimator '%s'", getName().c_str(), address.c_str());
+     882         117 :       estimator_list_.push_back(state_estimator_loader_->createInstance(address.c_str()));
+     883             :     }
+     884           0 :     catch (pluginlib::CreateClassException& ex1) {
+     885           0 :       ROS_ERROR("[%s]: CreateClassException for the estimator '%s'", getName().c_str(), address.c_str());
+     886           0 :       ROS_ERROR("[%s]: Error: %s", getName().c_str(), ex1.what());
+     887           0 :       ros::shutdown();
+     888             :     }
+     889           0 :     catch (pluginlib::PluginlibException& ex) {
+     890           0 :       ROS_ERROR("[%s]: PluginlibException for the estimator '%s'", getName().c_str(), address.c_str());
+     891           0 :       ROS_ERROR("[%s]: Error: %s", getName().c_str(), ex.what());
+     892           0 :       ros::shutdown();
+     893             :     }
+     894             :   }
+     895             : 
+     896             :   /*//{ load agl estimator plugins */
+     897         109 :   param_loader.loadParam("agl_height_estimator", est_alt_agl_name_);
+     898         109 :   is_using_agl_estimator_ = est_alt_agl_name_ != "";
+     899         109 :   ROS_WARN_COND(!is_using_agl_estimator_, "[%s]: not using AGL estimator for min height safe checking", getName().c_str());
+     900             : 
+     901             : 
+     902         109 :   if (is_using_agl_estimator_) {
+     903             : 
+     904          80 :     agl_estimator_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::AglEstimator>>("mrs_uav_managers", "mrs_uav_managers::AglEstimator");
+     905             : 
+     906             :     // load the estimator parameters
+     907         160 :     std::string address;
+     908          80 :     param_loader.loadParam(est_alt_agl_name_ + "/address", address);
+     909             : 
+     910             :     try {
+     911          80 :       ROS_INFO("[%s]: loading the estimator '%s'", getName().c_str(), address.c_str());
+     912          80 :       est_alt_agl_ = agl_estimator_loader_->createInstance(address.c_str());
+     913             :     }
+     914           0 :     catch (pluginlib::CreateClassException& ex1) {
+     915           0 :       ROS_ERROR("[%s]: CreateClassException for the estimator '%s'", getName().c_str(), address.c_str());
+     916           0 :       ROS_ERROR("[%s]: Error: %s", getName().c_str(), ex1.what());
+     917           0 :       ros::shutdown();
+     918             :     }
+     919           0 :     catch (pluginlib::PluginlibException& ex) {
+     920           0 :       ROS_ERROR("[%s]: PluginlibException for the estimator '%s'", getName().c_str(), address.c_str());
+     921           0 :       ROS_ERROR("[%s]: Error: %s", getName().c_str(), ex.what());
+     922           0 :       ros::shutdown();
+     923             :     }
+     924             :   }
+     925             :   /*//}*/
+     926             : 
+     927         109 :   ROS_INFO("[%s]: estimators were loaded", getName().c_str());
+     928             :   /*//}*/
+     929             : 
+     930             :   /*//{ check whether initial estimator was loaded */
+     931         109 :   param_loader.loadParam("initial_state_estimator", initial_estimator_name_);
+     932         109 :   bool initial_estimator_found = false;
+     933         109 :   for (auto estimator : estimator_list_) {
+     934         109 :     if (estimator->getName() == initial_estimator_name_) {
+     935         109 :       initial_estimator_      = estimator;
+     936         109 :       initial_estimator_found = true;
+     937         109 :       break;
+     938             :     }
+     939             :   }
+     940             : 
+     941         109 :   if (!initial_estimator_found) {
+     942           0 :     ROS_ERROR("[%s]: initial estimator %s could not be found among loaded estimators. shutting down", getName().c_str(), initial_estimator_name_.c_str());
+     943           0 :     ros::shutdown();
+     944             :   }
+     945             :   /*//}*/
+     946             : 
+     947             :   /*//{ initialize estimators */
+     948         226 :   for (auto estimator : estimator_list_) {
+     949             : 
+     950             :     // create private handlers
+     951         234 :     std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> ph = std::make_shared<mrs_uav_managers::estimation_manager::PrivateHandlers_t>();
+     952             : 
+     953         117 :     ph->loadConfigFile = boost::bind(&EstimationManager::loadConfigFile, this, _1);
+     954         117 :     ph->param_loader   = std::make_unique<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, estimator->getName()), "EstimationManager/" + estimator->getName());
+     955             : 
+     956         117 :     if (_custom_config_ != "") {
+     957         117 :       ph->param_loader->addYamlFile(_custom_config_);
+     958             :     }
+     959             : 
+     960         117 :     if (_platform_config_ != "") {
+     961         117 :       ph->param_loader->addYamlFile(_platform_config_);
+     962             :     }
+     963             : 
+     964         117 :     if (_world_config_ != "") {
+     965         117 :       ph->param_loader->addYamlFile(_world_config_);
+     966             :     }
+     967             : 
+     968             :     try {
+     969         117 :       ROS_INFO("[%s]: initializing the estimator '%s'", getName().c_str(), estimator->getName().c_str());
+     970         117 :       estimator->initialize(nh_, ch_, ph);
+     971             :     }
+     972           0 :     catch (std::runtime_error& ex) {
+     973           0 :       ROS_ERROR("[%s]: exception caught during estimator initialization: '%s'", getName().c_str(), ex.what());
+     974           0 :       ros::shutdown();
+     975             :     }
+     976             : 
+     977         117 :     if (!estimator->isCompatibleWithHwApi(hw_api_capabilities)) {
+     978           0 :       ROS_ERROR("[%s]: estimator %s is not compatible with the hw api. Shutting down.", getName().c_str(), estimator->getName().c_str());
+     979           0 :       ros::shutdown();
+     980             :     }
+     981             :   }
+     982             : 
+     983             :   // | ----------- agl height estimator initialization ---------- |
+     984         109 :   if (is_using_agl_estimator_) {
+     985             : 
+     986         160 :     std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> ph = std::make_shared<mrs_uav_managers::estimation_manager::PrivateHandlers_t>();
+     987             : 
+     988          80 :     ph->loadConfigFile = boost::bind(&EstimationManager::loadConfigFile, this, _1);
+     989          80 :     ph->param_loader   = std::make_unique<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, est_alt_agl_->getName()), "EstimationManager/" + est_alt_agl_->getName());
+     990             : 
+     991          80 :     if (_custom_config_ != "") {
+     992          80 :       ph->param_loader->addYamlFile(_custom_config_);
+     993             :     }
+     994             : 
+     995          80 :     if (_platform_config_ != "") {
+     996          80 :       ph->param_loader->addYamlFile(_platform_config_);
+     997             :     }
+     998             : 
+     999          80 :     if (_world_config_ != "") {
+    1000          80 :       ph->param_loader->addYamlFile(_world_config_);
+    1001             :     }
+    1002             : 
+    1003             :     try {
+    1004          80 :       ROS_INFO("[%s]: initializing the estimator '%s'", getName().c_str(), est_alt_agl_->getName().c_str());
+    1005          80 :       est_alt_agl_->initialize(nh_, ch_, ph);
+    1006             :     }
+    1007           0 :     catch (std::runtime_error& ex) {
+    1008           0 :       ROS_ERROR("[%s]: exception caught during estimator initialization: '%s'", getName().c_str(), ex.what());
+    1009           0 :       ros::shutdown();
+    1010             :     }
+    1011             : 
+    1012          80 :     if (!est_alt_agl_->isCompatibleWithHwApi(hw_api_capabilities)) {
+    1013           0 :       ROS_ERROR("[%s]: estimator %s is not compatible with the hw api. Shutting down.", getName().c_str(), est_alt_agl_->getName().c_str());
+    1014           0 :       ros::shutdown();
+    1015             :     }
+    1016             :   }
+    1017             : 
+    1018         109 :   ROS_INFO("[%s]: estimators were initialized", getName().c_str());
+    1019             : 
+    1020             :   /*//}*/
+    1021             : 
+    1022             :   /*//{ initialize publishers */
+    1023         109 :   ph_uav_state_    = mrs_lib::PublisherHandler<mrs_msgs::UavState>(nh_, "uav_state_out", 10);
+    1024         109 :   ph_odom_main_    = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh_, "odom_main_out", 10);
+    1025         109 :   ph_innovation_   = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh_, "innovation_out", 10);
+    1026         109 :   ph_diagnostics_  = mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics>(nh_, "diagnostics_out", 10);
+    1027         109 :   ph_max_flight_z_ = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "max_flight_z_agl_out", 10);
+    1028         109 :   ph_altitude_agl_ = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "height_agl_out", 10);
+    1029             : 
+    1030             :   /*//}*/
+    1031             : 
+    1032             :   /*//{ initialize timers */
+    1033         109 :   timer_publish_ = nh_.createTimer(ros::Rate(ch_->desired_uav_state_rate), &EstimationManager::timerPublish, this);
+    1034             : 
+    1035         109 :   timer_publish_diagnostics_ = nh_.createTimer(ros::Rate(ch_->desired_diagnostics_rate), &EstimationManager::timerPublishDiagnostics, this);
+    1036             : 
+    1037         109 :   timer_check_health_ = nh_.createTimer(ros::Rate(ch_->desired_uav_state_rate), &EstimationManager::timerCheckHealth, this);
+    1038             :   /*//}*/
+    1039             : 
+    1040             :   /*//{ initialize service clients */
+    1041             : 
+    1042         109 :   srvch_failsafe_.initialize(nh_, "failsafe_out");
+    1043             : 
+    1044             :   /*//}*/
+    1045             : 
+    1046             :   /*//{ initialize service servers */
+    1047         109 :   srvs_change_estimator_ = nh_.advertiseService("change_estimator_in", &EstimationManager::callbackChangeEstimator, this);
+    1048         109 :   srvs_reset_estimator_ = nh_.advertiseService("reset_estimator_in", &EstimationManager::callbackResetEstimator, this);
+    1049         109 :   srvs_toggle_callbacks_ = nh_.advertiseService("toggle_service_callbacks_in", &EstimationManager::callbackToggleServiceCallbacks, this);
+    1050             :   /*//}*/
+    1051             : 
+    1052             :   /*//{ initialize scope timer */
+    1053         109 :   param_loader.loadParam("scope_timer/enabled", ch_->scope_timer.enabled);
+    1054         218 :   std::string       filepath;
+    1055         218 :   const std::string time_logger_filepath = ros::package::getPath(package_name_) + "/scope_timer/scope_timer.txt";
+    1056         109 :   ch_->scope_timer.logger                = std::make_shared<mrs_lib::ScopeTimerLogger>(time_logger_filepath, ch_->scope_timer.enabled);
+    1057             :   /*//}*/
+    1058             : 
+    1059         109 :   if (!param_loader.loadedSuccessfully()) {
+    1060           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getName().c_str());
+    1061           0 :     ros::shutdown();
+    1062             :   }
+    1063             : 
+    1064         109 :   sm_->changeState(StateMachine::INITIALIZED_STATE);
+    1065             : 
+    1066         109 :   ROS_INFO("[%s]: initialized", getName().c_str());
+    1067         109 : }
+    1068             : /*//}*/
+    1069             : 
+    1070             : // | -------------------- service callbacks ------------------- |
+    1071             : 
+    1072             : /*//{ callbackChangeEstimator() */
+    1073           5 : bool EstimationManager::callbackChangeEstimator(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+    1074             : 
+    1075           5 :   if (!sm_->isInitialized()) {
+    1076           0 :     return false;
+    1077             :   }
+    1078             : 
+    1079             :   // enable switching out from vins_kickoff estimator during takeoff
+    1080           5 :   if (!callbacks_enabled_ && active_estimator_->getName() != "vins_kickoff") {
+    1081           0 :     res.success = false;
+    1082           0 :     res.message = ("Service callbacks are disabled");
+    1083           0 :     ROS_WARN("[%s]: Ignoring service call. Callbacks are disabled.", getName().c_str());
+    1084           0 :     return true;
+    1085             :   }
+    1086             : 
+    1087             :   // switching into these estimators during flight is dangerous with realhw, so we don't allow it 
+    1088           5 :   if (req.value == "dummy" || req.value == "ground_truth" || req.value == "vins_kickoff") {
+    1089           0 :     res.success = false;
+    1090           0 :     std::stringstream ss;
+    1091           0 :     ss << "Switching to " << req.value << " estimator is not allowed.";
+    1092           0 :     res.message = ss.str();
+    1093           0 :     ROS_WARN("[%s]: Switching to %s estimator is not allowed.", getName().c_str(), req.value.c_str());
+    1094           0 :     return true;
+    1095             :   }
+    1096             : 
+    1097             :   // we do not want the switch to be disturbed by a service call
+    1098           5 :   callbacks_enabled_ = false;
+    1099             : 
+    1100           5 :   bool                                                target_estimator_found = false;
+    1101           5 :   boost::shared_ptr<mrs_uav_managers::StateEstimator> target_estimator;
+    1102          12 :   for (auto estimator : estimator_list_) {
+    1103          12 :     if (estimator->getName() == req.value) {
+    1104           5 :       target_estimator       = estimator;
+    1105           5 :       target_estimator_found = true;
+    1106           5 :       break;
+    1107             :     }
+    1108             :   }
+    1109             : 
+    1110           5 :   if (target_estimator_found) {
+    1111             : 
+    1112           5 :     if (target_estimator->isRunning()) {
+    1113           5 :       sm_->changeState(StateMachine::ESTIMATOR_SWITCHING_STATE);
+    1114           5 :       switchToEstimator(target_estimator);
+    1115           5 :       sm_->changeToPreSwitchState();
+    1116             :     } else {
+    1117           0 :       ROS_WARN("[%s]: Switch to not running estimator %s requested", getName().c_str(), req.value.c_str());
+    1118           0 :       res.success = false;
+    1119           0 :       res.message = ("Requested estimator is not running");
+    1120           0 :       return true;
+    1121             :     }
+    1122             : 
+    1123             :   } else {
+    1124           0 :     ROS_WARN("[%s]: Switch to invalid estimator %s requested", getName().c_str(), req.value.c_str());
+    1125           0 :     res.success = false;
+    1126           0 :     res.message = ("Not a valid estimator type");
+    1127           0 :     return true;
+    1128             :   }
+    1129             : 
+    1130           5 :   res.success = true;
+    1131           5 :   res.message = "Estimator switch successful";
+    1132             : 
+    1133             :   // allow service calllbacks after switch again
+    1134           5 :   callbacks_enabled_ = true;
+    1135             : 
+    1136           5 :   return true;
+    1137             : }
+    1138             : /*//}*/
+    1139             : 
+    1140             : /*//{ callbackResetEstimator() */
+    1141           0 : bool EstimationManager::callbackResetEstimator(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+    1142             : 
+    1143           0 :   if (!sm_->isInitialized()) {
+    1144           0 :     return false;
+    1145             :   }
+    1146             : 
+    1147           0 :   if (!callbacks_enabled_) {
+    1148           0 :     res.success = false;
+    1149           0 :     res.message = ("Service callbacks are disabled");
+    1150           0 :     ROS_WARN("[%s]: Ignoring service call. Callbacks are disabled.", getName().c_str());
+    1151           0 :     return true;
+    1152             :   }
+    1153             : 
+    1154             : 
+    1155           0 :   bool                                                target_estimator_found = false;
+    1156           0 :   boost::shared_ptr<mrs_uav_managers::StateEstimator> target_estimator;
+    1157           0 :   for (auto estimator : estimator_list_) {
+    1158           0 :     if (estimator->getName() == req.value) {
+    1159           0 :       target_estimator       = estimator;
+    1160           0 :       target_estimator_found = true;
+    1161           0 :       break;
+    1162             :     }
+    1163             :   }
+    1164             : 
+    1165           0 :   if (target_estimator_found) {
+    1166             : 
+    1167             : 
+    1168           0 :     if (target_estimator->getName() == active_estimator_->getName()) {
+    1169           0 :       res.success = false;
+    1170           0 :       res.message = ("Cannot reset active estimator");
+    1171           0 :       ROS_WARN("[%s]: Ignoring service call. Cannot reset active estimator.", getName().c_str());
+    1172           0 :       return true;
+    1173             :     }
+    1174             : 
+    1175           0 :       target_estimator->reset();
+    1176           0 :       ROS_INFO("[EstimationManager]: Estimator %s reset", target_estimator->getName().c_str());
+    1177             : 
+    1178           0 :       double t_wait_left = 5;
+    1179           0 :       while (t_wait_left > 0) {
+    1180           0 :         ROS_INFO("[EstimationManager]: Attempting starting %s estimator", target_estimator->getName().c_str());
+    1181           0 :         target_estimator->start();
+    1182             : 
+    1183           0 :         if (target_estimator->isRunning()) {
+    1184           0 :           ROS_INFO("[EstimationManager]: Reset of %s estimator successful", target_estimator->getName().c_str());
+    1185           0 :           break;
+    1186             :         } 
+    1187             :         
+    1188           0 :         const double start_period = 1.0;
+    1189           0 :         ros::Duration(start_period).sleep();
+    1190           0 :         t_wait_left -= start_period;
+    1191             : 
+    1192             :       }
+    1193             : 
+    1194             :   } else {
+    1195           0 :     ROS_WARN("[%s]: Reset of invalid estimator %s requested", getName().c_str(), req.value.c_str());
+    1196           0 :     res.success = false;
+    1197           0 :     res.message = ("Not a valid estimator type");
+    1198           0 :     return true;
+    1199             :   }
+    1200             : 
+    1201           0 :   res.success = true;
+    1202           0 :   res.message = "Estimator reset successful";
+    1203             : 
+    1204           0 :   return true;
+    1205             : }
+    1206             : /*//}*/
+    1207             : 
+    1208             : /* //{ callbackToggleServiceCallbacks() */
+    1209         144 : bool EstimationManager::callbackToggleServiceCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    1210             : 
+    1211         144 :   if (!sm_->isInitialized()) {
+    1212           0 :     ROS_ERROR("[%s]: service for toggling callbacks is not available before initialization.", getName().c_str());
+    1213           0 :     return false;
+    1214             :   }
+    1215             : 
+    1216         144 :   callbacks_disabled_by_service_ = !req.data;
+    1217             : 
+    1218         144 :   res.success = true;
+    1219         144 :   res.message = (callbacks_disabled_by_service_ ? "Service callbacks disabled" : "Service callbacks enabled");
+    1220             : 
+    1221         144 :   if (callbacks_disabled_by_service_) {
+    1222             : 
+    1223          43 :     ROS_INFO("[%s]: Service callbacks disabled.", getName().c_str());
+    1224             : 
+    1225             :   } else {
+    1226             : 
+    1227         101 :     ROS_INFO("[%s]: Service callbacks enabled", getName().c_str());
+    1228             :   }
+    1229             : 
+    1230         144 :   return true;
+    1231             : }
+    1232             : 
+    1233             : //}
+    1234             : 
+    1235             : 
+    1236             : // --------------------------------------------------------------
+    1237             : // |                        other methods                       |
+    1238             : // --------------------------------------------------------------
+    1239             : //
+    1240             : /*//{ switchToHealthyEstimator() */
+    1241           0 : bool EstimationManager::switchToHealthyEstimator() {
+    1242             : 
+    1243             :   // available estimators should be specified in decreasing priority order in config file
+    1244           0 :   for (auto estimator : estimator_list_) {
+    1245           0 :     if (estimator->isRunning()) {
+    1246           0 :       switchToEstimator(estimator);
+    1247           0 :       return true;
+    1248             :     }
+    1249             :   }
+    1250           0 :   return false;  // no other estimator is running
+    1251             : }
+    1252             : /*//}*/
+    1253             : 
+    1254             : /*//{ switchToEstimator() */
+    1255           5 : void EstimationManager::switchToEstimator(const boost::shared_ptr<mrs_uav_managers::StateEstimator>& target_estimator) {
+    1256             : 
+    1257           5 :   std::scoped_lock lock(mutex_active_estimator_);
+    1258           5 :   ROS_INFO("[%s]: switching estimator from %s to %s", getName().c_str(), active_estimator_->getName().c_str(), target_estimator->getName().c_str());
+    1259           5 :   active_estimator_ = target_estimator;
+    1260           5 :   estimator_switch_count_++;
+    1261           5 : }
+    1262             : /*//}*/
+    1263             : 
+    1264             : /*//{ callFailsafeService() */
+    1265           0 : bool EstimationManager::callFailsafeService() {
+    1266           0 :   std_srvs::Trigger srv_out;
+    1267           0 :   return srvch_failsafe_.call(srv_out);
+    1268             : }
+    1269             : /*//}*/
+    1270             : 
+    1271             : /*//{ getName() */
+    1272        3631 : std::string EstimationManager::getName() const {
+    1273        3631 :   return nodelet_name_;
+    1274             : }
+    1275             : /*//}*/
+    1276             : 
+    1277             : /* loadConfigFile() //{ */
+    1278             : 
+    1279           0 : bool EstimationManager::loadConfigFile(const std::string& file_path) {
+    1280             : 
+    1281           0 :   const std::string name_space = nh_.getNamespace() + "/";
+    1282             : 
+    1283           0 :   ROS_INFO("[%s]: loading '%s' under the namespace '%s'", getName().c_str(), file_path.c_str(), name_space.c_str());
+    1284             : 
+    1285             :   // load the user-requested file
+    1286             :   {
+    1287           0 :     std::string command = "rosparam load " + file_path + " " + name_space;
+    1288           0 :     int         result  = std::system(command.c_str());
+    1289             : 
+    1290           0 :     if (result != 0) {
+    1291           0 :       ROS_ERROR("[%s]: failed to load '%s'", getName().c_str(), file_path.c_str());
+    1292           0 :       return false;
+    1293             :     }
+    1294             :   }
+    1295             : 
+    1296             :   // load the platform config
+    1297           0 :   if (_platform_config_ != "") {
+    1298           0 :     std::string command = "rosparam load " + _platform_config_ + " " + name_space;
+    1299           0 :     int         result  = std::system(command.c_str());
+    1300             : 
+    1301           0 :     if (result != 0) {
+    1302           0 :       ROS_ERROR("[%s]: failed to load the platform config file '%s'", getName().c_str(), _platform_config_.c_str());
+    1303           0 :       return false;
+    1304             :     }
+    1305             :   }
+    1306             : 
+    1307             :   // load the custom config
+    1308           0 :   if (_custom_config_ != "") {
+    1309           0 :     std::string command = "rosparam load " + _custom_config_ + " " + name_space;
+    1310           0 :     int         result  = std::system(command.c_str());
+    1311             : 
+    1312           0 :     if (result != 0) {
+    1313           0 :       ROS_ERROR("[%s]: failed to load the custom config file '%s'", getName().c_str(), _custom_config_.c_str());
+    1314           0 :       return false;
+    1315             :     }
+    1316             :   }
+    1317             : 
+    1318           0 :   return true;
+    1319             : }
+    1320             : 
+    1321             : //}
+    1322             : 
+    1323             : }  // namespace estimation_manager
+    1324             : 
+    1325             : }  // namespace mrs_uav_managers
+    1326             : 
+    1327             : #include <pluginlib/class_list_macros.h>
+    1328         109 : 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..4161850921 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.overview.html @@ -0,0 +1,352 @@ + + + + + + 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 + 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..7bc931a74d24b1f80677c0bc124e35ade4d2928d GIT binary patch literal 4637 zcmV+&65{QNP)TN+ z`=woLeRt?ulQt6g9sS*%5()r~80|Ff!twG^7wrU9-G`3S!MG&QNK$-#|1x_(D~;xcY0OBgq0zOzG`c=#zKes#Ssvub z0Agl>K;eUD$c^>H{uaO3KX>O^9-M!~^}=?2IoG$~74D8eX`LB7;PH~&D&OLMKE8ji z?(xU)Xa1*s-S6z~sT@6+4K}w@Tu$Q{CRJ?w1k(*b`l6GOB7O1pG^bJOx{KmQl`1ha zWo#_D%q!NJF`Lz_g|uUQKA$hww(!sT{%1X2R`=!JcEG)@ue@zf&^*RB+*5CFz^wq{ z0qxoYltNfJTEvrh-Xw|-@j#Kh{GV-3j;s4_eFeakh)u+363@l9VjQ9B%91?fn7j%w zZOIDev`3MTm(F&>ym8w0I)oB2mIQeKS`^JOBN`ISwaKWo3OtuzN#M~@32&|z_-iE; za{&Rr4FBShQN3niDx?p`n+_xMYXvTgTNXH$_T>WKzcoyS$1j`;&$Jeb>tJLiE}H9x zU(=h{l_RrajMoX=BQyb;{F%0?pe@Cpvpt5XF!Y`ZUNGT}B%Y7Y8FkXw=FA3+{okA^ z3VbcnNCnRP$)$4!WBhFbZ&(!=kzZ`2ov?tv&&btAnJ;qlkW&f>fKG@KJ#+4WR*&MM-#j}B=h=*U6L93bwx7&rrJrl~{?*Q1Pth{w*f^n2tcU<$cHGg{Fg7qs8sK0BL`O|$XdGaC?0ic?ES;UXUdz*=^I=orOf$xNru{$3 zK6W_B2?vlR_Nf5~uUSqK04iZL>J{u)CDR6VtIamC=h$4H=wcC3HI3%%rwfys${-#v z=;LJw`z(dvpj!v@Py?7Vg`xKm>_Ro&K4A@F#6wGBwRgl2)RIsF+BEZYa3t&6X z{?+(0@klDc0AACz+``mQ0Np4hz-zjWb5a8wK*tXe(5ulMe7Y8Jce*Wrb)zM79p702 zYbTP35n-*yXc5El>pkl$%l~fdS|b2={Xl;=G)OV)T1G&-KF>GhTFLpk5w6ei478_I zcoM%Ak12;8$s2HB0Pp22`8pbfePM~l`;{wdiN;1bdiHWa&+|YqOBe?VSiubceI|cd z%w&`d7{Ake3?%hamw_+oH0PsK9`7 zx0+7@|2P5`8L95tjIq3LEjmc8fEF6ftt9Y=(<2+#4~>~22Q-ma$ILzN>`XHjRZ>84 zk~el8tVk^mg0Q+aMaxUA3iujZ=UvtyDv1;UCKGDDF({xGPodr zRe2F;8elC(5TIqTU%)j%c@`sEuVBU)C!s3K3P5YuhffjQhx-s*pxq6)4>73r_9?(v zXY9kzH>05#Z>-Q*8qJfdvA{T6MtT+)-vP?Iw}HU?H=NP#Wf^{ zkKr`MFxuTpcaoY)=Af19tVJ>0K-$F?5-nmmOUk==IY2{3#m!inQj^R{nbArc6OX$z zXa9Cu;DP*ijJ2z4Qid>CPIt728y#yc-#Y@1P`OmDO$dSZfIOEiXw#Oo=&Q6x&?3Oh zx@YNZk@}Qyw>cU=$IMBt^FhT!sH7O#nHi~=dA)6R;{2@U0jID~NQdDLutJVhYa|*w zFcXLE_$MFGvt9d|Cl+vqaLhm+ZFe+}U*3oieG{2oS=!DdxXpbmZq5uNPLUaE4Krq- zWco9MDLY;mj6D^1qD-SM@~-7{JY&~w;+e7mY2q3winuHWrO5P(@}6g~2?DZfkJ%yb z7A#1iAqi0S?Big8dll+O=FhXoVqiUc&cFXE-|Ww`|MTqIwMWpRAK}^mKUc0Z;$bBu z?>@2+GPtIJQKV!@bFO(wVZh^Wd7r&5HGLtg1~2feA0D(a>S{UT$@1|5|7~3(j97*m z*I3vmyWT%VggT9An001kUfRSHzN31^bj<+rG%knU;0+KDI5S;~TL8d&6W3-Zm#ITy zSP`sQ=Aeyz(&w$M)kqx_XTGMuHRah`vnE%;FP|H;ic-J>m1H%LoKQRPYnN}3Wcu2IeIEbjEaDQI=ml#G{SY1 zf1-|)X3GtJ4yyt<7IK9^*O;Bzy-x6>mt*Q#$7o9!cq1guBz4sw2a|$NJfvc5W?>h) z?s9wR#S2XyL7G{fM17=AITX*N)#f+3Tw;Z<$7nz|)(L#iI3HrVm6C9Je}ptFb&%xf zHI}42XYx!y@MARDQ@Ds>aVNqEBbZnGomaF^XQ$|BfeThDYZlo)BzX^ivp&;9y~e#|rMUUqOUh*0oWN z9d2r{A3z|1Z}1}!KH)l?xva@m1ot38gsXIEv|T*;z-+I0XBco*dt^1G<)cyHMCgnS z=L2Y(qe&B3KIdqr1n933PreL)OgxHDk>w!$ey*7t@-ugar*X_30KXj$*%{Lzu$yx! z(gMhwave@q8Kd{tkWT>+ z1)L~UC5`d#cjZ$6`T5TP_S}f8L`pO=(d&*hzO8@~X%lGVj-@-F0>~0l0O?7EjFFsa zDsZzI;{p$8jg7&`pJCTVhU4Rg$|XF;kx}>Hk(vE9u_6RiZvKku$mujmX=G+Vt9XPV zO!hkL3o9_P#{O7up*4UBey@M3lCaD$yp);Q7>pM&)8)DLalO`Tlwp4z zrb0ef7PltT$;DuE3LZ{f-Ak7X7|ca1#;)NcyvE}G8*3J=?gWRhg2rCfOmPW)K;zGv z{co&Uw_riY!;J|aLBHahS^7A`@rrCWy$8nf+QK+4-h=8Dr2OleY8P|@fwvtPFi|-w zF%sgX%wj~t0~!SWJy}iiW8m;4bagPBn~1AFv*~uu)h`pM6w1B6s468F4tjo}%@>|( z&K2@gk{sXrKLE&GsE}GyDOoAWMm%_-fYv!aEi`*lQZZoed}PId{<|JNMT1hXIyon<2{YSq(*E+!Qg{S#7iM}SY5N!) zC@BE}_Naq>cSv&kGtH&0n>75-G`G{(-!%ImJjeB)X=d6Kx&B3_ncb2y8K|E>(lBiS zJjt@X^|^(WfSLuzfU|1q;mMf_IM)Dv$H=HB@uP*8tb$f!hEJ&C1YVC(J>HvpA607c zaC4*UmUMy7w`h;~6hO;%gMh*qrVNb|#^a~XW38r~R%VCoREAf9#_a5q=YGbgDgLiH zF?C5{WeDP~B9O9>!DYT*l+8^W9T{UI>$O}d^4aJ3;OC*C4d2OU$ssnX9fVKOOTIO7BE%q`#|(7 zIM3;v%e^m(r&q1Ldz2{fYr8nEj|AmHXoU zudb_Ijv{$b2iBW>S6lQ5y`vc)8nVS-xB((JeFZO;1%zAnl`bzV;7hsaD*%?n{n=1F zo`u4-bDkL^jpnYiOYf+SS!36)IMTWt;p|E|NA%B#mjY&2xV6Fm_DP@O`NWf<$Tg5# zRCSm@Sz($3*N1+~jG;@D-+;RIa71pAQgD%lh3zV4sG^AmfFp7Z8{R-<1T=MRQHv_2 zcp??ucrrMFvlJ)5IQtZ4ENv(AYBSa1UT-)N5lx2Q*^k9*Ia$j--dO>x|%%d-n;_G~yuNITtlsr>6x&M*xXND-gfbA{S{VQO>AWq1Fa zxzvdd*|QHJPa~6f(WT1w+TF0ZU2RY*T4s5uAxePg&$7FB zhkA@=8gUFj4g!cV0OCzmxC*Hl4;^{LFWh1EqjkF{Z)kF_S%B=Z1+eO4u=VQ}SL>B^ zK0dj+!b4U~=Y-WY%a~}njEJC=0FlKzuFyxxiXcNnd`czl`e+J`pF9z_-e$|sYp&8R z{`OM+=3Cl6>eK5B6t{er3#XZ(lz+_0^w zOcU>R>$WKY;TCguUNAs(e5hNw22fBS50ly0KNE>zo4GpP*VVsyIj-Gg+CP#4Su~w$ zLL_e6u@#<)l2y>Iu)Y?b+S~RiA1)9HLwHx`*%NXo=9`0H0W*YX7{6@@xA-Ydh#xkD z`Urv{R7bEhglQsv;}DV&{24=deFVweuHP39canB^ImYue#tj!}EFkBtGj0A4$ja~7 TmZmdu00000NkvXXu0mjf1AEB> literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/src/estimation_manager/estimator.cpp.func-sort-c.html b/mrs_uav_managers/src/estimation_manager/estimator.cpp.func-sort-c.html new file mode 100644 index 0000000000..8fc0dc6fbd --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimator.cpp.func-sort-c.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:638871.6 %
Date:2024-12-17 22:35:29Functions: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)2500
mrs_uav_managers::Estimator::setCurrentSmState(mrs_uav_managers::estimation_manager::SMStates_t const&)2500
mrs_uav_managers::Estimator::getCurrentSmStateString[abi:cxx11]() const2500
mrs_uav_managers::Estimator::getPrintName[abi:cxx11]() const3720
mrs_uav_managers::Estimator::getSmStateString[abi:cxx11](mrs_uav_managers::estimation_manager::SMStates_t const&) const5000
mrs_uav_managers::Estimator::getMaxFlightZ() const19516
mrs_uav_managers::Estimator::isStarted() const40484
mrs_uav_managers::Estimator::getAccGlobal(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, double)322303
mrs_uav_managers::Estimator::getAccGlobal(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&, double)322418
mrs_uav_managers::Estimator::isReady() const358290
mrs_uav_managers::Estimator::getName[abi:cxx11]() const673681
mrs_uav_managers::Estimator::getFrameId[abi:cxx11]() const767732
mrs_uav_managers::Estimator::isMitigatingJump()806890
mrs_uav_managers::Estimator::publishDiagnostics() const1070808
mrs_uav_managers::Estimator::isError() const1469881
mrs_uav_managers::Estimator::isRunning() const1813544
mrs_uav_managers::Estimator::isInitialized() const2817454
mrs_uav_managers::Estimator::isInState(mrs_uav_managers::estimation_manager::SMStates_t const&) const8397891
mrs_uav_managers::Estimator::getCurrentSmState() const9206198
+
+
+ + + +
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..9c7936c46f --- /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:638871.6 %
Date:2024-12-17 22:35:29Functions: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)2500
mrs_uav_managers::Estimator::getAccGlobal(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, double)322303
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)322418
mrs_uav_managers::Estimator::getHeadingRate(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)0
mrs_uav_managers::Estimator::isMitigatingJump()806890
mrs_uav_managers::Estimator::setCurrentSmState(mrs_uav_managers::estimation_manager::SMStates_t const&)2500
mrs_uav_managers::Estimator::getFrameId[abi:cxx11]() const767732
mrs_uav_managers::Estimator::getPrintName[abi:cxx11]() const3720
mrs_uav_managers::Estimator::getMaxFlightZ() const19516
mrs_uav_managers::Estimator::isInitialized() const2817454
mrs_uav_managers::Estimator::getSmStateString[abi:cxx11](mrs_uav_managers::estimation_manager::SMStates_t const&) const5000
mrs_uav_managers::Estimator::getCurrentSmState() const9206198
mrs_uav_managers::Estimator::publishDiagnostics() const1070808
mrs_uav_managers::Estimator::getCurrentSmStateString[abi:cxx11]() const2500
mrs_uav_managers::Estimator::getName[abi:cxx11]() const673681
mrs_uav_managers::Estimator::getType[abi:cxx11]() const0
mrs_uav_managers::Estimator::isError() const1469881
mrs_uav_managers::Estimator::isReady() const358290
mrs_uav_managers::Estimator::isInState(mrs_uav_managers::estimation_manager::SMStates_t const&) const8397891
mrs_uav_managers::Estimator::isRunning() const1813544
mrs_uav_managers::Estimator::isStarted() const40484
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..bfe57ba088 --- /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:638871.6 %
Date:2024-12-17 22:35:29Functions: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        2500 : bool Estimator::changeState(SMStates_t new_state) {
+       9             : 
+      10        2500 :   if (new_state == getCurrentSmState()) {
+      11           0 :     return true;
+      12             :   }
+      13             : 
+      14        2500 :   previous_sm_state_ = getCurrentSmState();
+      15        2500 :   setCurrentSmState(new_state);
+      16             : 
+      17        2500 :   ROS_INFO("[%s]: Switching sm state %s -> %s", getPrintName().c_str(), getSmStateString(previous_sm_state_).c_str(), getCurrentSmStateString().c_str());
+      18        2500 :   return true;
+      19             : }
+      20             : /*//}*/
+      21             : 
+      22             : /*//{ isInState() */
+      23     8397891 : bool Estimator::isInState(const SMStates_t& state_in) const {
+      24     8397891 :   return state_in == getCurrentSmState();
+      25             : }
+      26             : /*//}*/
+      27             : 
+      28             : /*//{ isInitialized() */
+      29     2817454 : bool Estimator::isInitialized() const {
+      30     2817454 :   return !isInState(UNINITIALIZED_STATE);
+      31             : }
+      32             : /*//}*/
+      33             : 
+      34             : /*//{ isReady() */
+      35      358290 : bool Estimator::isReady() const {
+      36      358290 :   return isInState(READY_STATE);
+      37             : }
+      38             : /*//}*/
+      39             : 
+      40             : /*//{ isStarted() */
+      41       40484 : bool Estimator::isStarted() const {
+      42       40484 :   return isInState(STARTED_STATE);
+      43             : }
+      44             : /*//}*/
+      45             : 
+      46             : /*//{ isRunning() */
+      47     1813544 : bool Estimator::isRunning() const {
+      48     1813544 :   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     1469881 : bool Estimator::isError() const {
+      60     1469881 :   return isInState(ERROR_STATE);
+      61             : }
+      62             : /*//}*/
+      63             : 
+      64             : /*//{ getCurrentSmState() */
+      65     9206198 : SMStates_t Estimator::getCurrentSmState() const {
+      66     9206198 :   return current_sm_state_;
+      67             : }
+      68             : /*//}*/
+      69             : 
+      70             : /*//{ setCurrentSmState() */
+      71        2500 : void Estimator::setCurrentSmState(const SMStates_t& new_state) {
+      72        2500 :   std::scoped_lock lock(mutex_current_state_);
+      73        2500 :   current_sm_state_ = new_state;
+      74        2500 : }
+      75             : /*//}*/
+      76             : 
+      77             : /*//{ getSmStateString() */
+      78        5000 : std::string Estimator::getSmStateString(const SMStates_t& state) const {
+      79        5000 :   return sm::state_names[state];
+      80             : }
+      81             : /*//}*/
+      82             : 
+      83             : /*//{ getCurrentSmStateName() */
+      84        2500 : std::string Estimator::getCurrentSmStateString(void) const {
+      85        5000 :   return getSmStateString(getCurrentSmState());
+      86             : }
+      87             : /*//}*/
+      88             : 
+      89             : /*//{ isMitigatingJump() */
+      90      806890 : bool Estimator::isMitigatingJump(void) {
+      91      806890 :   if (is_mitigating_jump_) {
+      92           3 :     is_mitigating_jump_ = false;
+      93           0 :     return true;
+      94             :   } else {
+      95      806846 :     return false;
+      96             :   }
+      97             : }
+      98             : /*//}*/
+      99             : 
+     100             : /*//{ getName() */
+     101      673681 : std::string Estimator::getName(void) const {
+     102      673681 :   return name_;
+     103             : }
+     104             : /*//}*/
+     105             : 
+     106             : /*//{ getPrintName() */
+     107        3720 : std::string Estimator::getPrintName(void) const {
+     108        7440 :   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      767732 : std::string Estimator::getFrameId(void) const {
+     120      767732 :   return ns_frame_id_;
+     121             : }
+     122             : /*//}*/
+     123             : 
+     124             : /*//{ getMaxFlightZ() */
+     125       19516 : double Estimator::getMaxFlightZ(void) const {
+     126       19516 :   return max_flight_z_;
+     127             : }
+     128             : /*//}*/
+     129             : 
+     130             : /*//{ publishDiagnostics() */
+     131     1070808 : void Estimator::publishDiagnostics() const {
+     132             : 
+     133     1070808 :   if (!ch_->debug_topics.diag) {
+     134     1070760 :     return;
+     135             :   }
+     136             : 
+     137           0 :   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      322418 : tf2::Vector3 Estimator::getAccGlobal(const mrs_msgs::EstimatorInput::ConstPtr& input_msg, const double hdg) {
+     158             : 
+     159      640859 :   geometry_msgs::Vector3Stamped acc_stamped;
+     160      322152 :   acc_stamped.vector = input_msg->control_acceleration;
+     161      322244 :   acc_stamped.header = input_msg->header;
+     162      638065 :   return getAccGlobal(acc_stamped, hdg);
+     163             : }
+     164             : 
+     165      322303 : tf2::Vector3 Estimator::getAccGlobal(const geometry_msgs::Vector3Stamped& acc_stamped, const double hdg) {
+     166             : 
+     167             :   // untilt the desired acceleration vector
+     168      641099 :   geometry_msgs::Vector3Stamped des_acc;
+     169      322095 :   geometry_msgs::Vector3        des_acc_untilted;
+     170      322199 :   des_acc.vector.x        = acc_stamped.vector.x;
+     171      322199 :   des_acc.vector.y        = acc_stamped.vector.y;
+     172      322199 :   des_acc.vector.z        = acc_stamped.vector.z;
+     173      322199 :   des_acc.header.frame_id = ch_->frames.ns_fcu;
+     174      322229 :   des_acc.header.stamp    = acc_stamped.header.stamp;
+     175      643935 :   auto response_acc       = ch_->transformer->transformSingle(des_acc, ch_->frames.ns_fcu_untilted);
+     176      322429 :   if (response_acc) {
+     177      322429 :     des_acc_untilted.x = response_acc.value().vector.x;
+     178      322429 :     des_acc_untilted.y = response_acc.value().vector.y;
+     179      322427 :     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      322428 :   const tf2::Vector3 des_acc_global = Support::rotateVecByHdg(des_acc_untilted, hdg);
+     187             : 
+     188      638314 :   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..f401921282a6e8acd39246b08a7b89c74383dc24 GIT binary patch literal 824 zcmV-81IPS{P)zrGivy3RaagYEM zi_T=hDp4|pU_36HC#XtBn*Fzdv-qzAxc|y)Bo|c!eK^Q%B#{_qT$I&eW}pdRctqA^ z_zcRL<=wJIcS2^(#o7R@SyvaVNy->&lFa*AUySe$?bg?SlQqHpBUm40E6TQihytZT zh08)!E@6eU|NHv2ta*PQ464Y**q5w3%szy5vsppSUgm-|VxpEz0vhw8py-kO<7Dieod7zzZj& z9VaOtI=F^2ywXC~$Pc-1tz32B&Zvi!&^m(zZZ~Z4?aEiK{XjeuCK!M<@9Wr;Wm(a^ z5qmSX^BSNxV_l8;8g0>_ulp{1zpo|dYe{ETvhMO4gEiytwjfd2OiD(V8p+yqmJflA z zkM5gC5EN@QzvF?JZQDBlqM?&}Ml2~iRLP=FAJ=cMbGpSV34X(lWMCACHXqHQg18)) z=kS->(VIeUN9AAQOviqR6&~Min88tv5a=IJw#-030xiRD0+)0RfVvw33yn3_1LyP{ zJ(`}s=MqO=TSbj@nxFjpkaCiiYk2$LRL@#jq(_#8_Sn)ib<8<`+51SfA0=6wF5CZ*pz-UL68qU~hRl zYXbBqL2Us&U*o&&zI@QVll0000 + + + + + + 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-12-17 22:35:29Functions: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&) const80
mrs_uav_managers::AglEstimator::publishAglHeight() const129683
mrs_uav_managers::AglEstimator::publishCovariance() const129683
+
+
+ + + +
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..ab98320e2e --- /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-12-17 22:35:29Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::AglEstimator::publishAglHeight() const129683
mrs_uav_managers::AglEstimator::publishCovariance() const129683
mrs_uav_managers::AglEstimator::isCompatibleWithHwApi(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&) const80
+
+
+ + + +
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..0340684411 --- /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-12-17 22:35:29Functions: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      129683 : void AglEstimator::publishAglHeight() const {
+       8      129683 :   ph_agl_height_.publish(mrs_lib::get_mutexed(mtx_agl_height_, agl_height_));
+       9      129683 : }
+      10             : /*//}*/
+      11             : 
+      12             : /*//{ publishCovariance() */
+      13      129683 : void AglEstimator::publishCovariance() const {
+      14             : 
+      15      129683 :   if (!ch_->debug_topics.covariance) {
+      16      129683 :     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          80 : bool AglEstimator::isCompatibleWithHwApi(const mrs_msgs::HwApiCapabilitiesConstPtr& hw_api_capabilities) const {
+      25             : 
+      26          80 :   ph_->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml");
+      27          80 :   ph_->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml");
+      28             : 
+      29          80 :   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          80 :   ph_->param_loader->loadParam("requires/gnss", requires_gnss);
+      35          80 :   ph_->param_loader->loadParam("requires/imu", requires_imu);
+      36          80 :   ph_->param_loader->loadParam("requires/distance_sensor", requires_distance_sensor);
+      37          80 :   ph_->param_loader->loadParam("requires/altitude", requires_altitude);
+      38          80 :   ph_->param_loader->loadParam("requires/magnetometer_heading", requires_magnetometer_heading);
+      39          80 :   ph_->param_loader->loadParam("requires/position", requires_position);
+      40          80 :   ph_->param_loader->loadParam("requires/orientation", requires_orientation);
+      41          80 :   ph_->param_loader->loadParam("requires/velocity", requires_velocity);
+      42          80 :   ph_->param_loader->loadParam("requires/angular_velocity", requires_angular_velocity);
+      43             : 
+      44          80 :   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          80 :   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          80 :   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          80 :   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          80 :   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          80 :   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          80 :   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          80 :   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          80 :   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          80 :   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          80 :   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..df364f9f08 --- /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-12-17 22:35:29Functions: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..bfb5a9c145 --- /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-12-17 22:35:29Functions: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..d221fc9a97 --- /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-12-17 22:35:29Functions: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..08c828b7e8 --- /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-12-17 22:35:29Functions: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..8c2510f373 --- /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-12-17 22:35:29Functions: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..7008c19b8d --- /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-12-17 22:35:29Functions: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..87bbb4aced --- /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-12-17 22:35:29Functions: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&) const117
mrs_uav_managers::StateEstimator::getInnovation() const176277
mrs_uav_managers::StateEstimator::getPoseCovariance() const176277
mrs_uav_managers::StateEstimator::getTwistCovariance() const176277
mrs_uav_managers::StateEstimator::getUavState()195352
mrs_uav_managers::StateEstimator::publishUavState() const211552
mrs_uav_managers::StateEstimator::publishOdom() const211556
mrs_uav_managers::StateEstimator::publishCovariance() const211556
mrs_uav_managers::StateEstimator::publishInnovation() const211556
mrs_uav_managers::StateEstimator::rotateQuaternionByHeading(geometry_msgs::Quaternion_<std::allocator<void> > const&, double) const428536
+
+
+ + + +
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..6cb25d5f64 --- /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-12-17 22:35:29Functions:1010100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::StateEstimator::getUavState()195352
mrs_uav_managers::StateEstimator::publishOdom() const211556
mrs_uav_managers::StateEstimator::getInnovation() const176277
mrs_uav_managers::StateEstimator::publishUavState() const211552
mrs_uav_managers::StateEstimator::getPoseCovariance() const176277
mrs_uav_managers::StateEstimator::publishCovariance() const211556
mrs_uav_managers::StateEstimator::publishInnovation() const211556
mrs_uav_managers::StateEstimator::getTwistCovariance() const176277
mrs_uav_managers::StateEstimator::isCompatibleWithHwApi(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&) const117
mrs_uav_managers::StateEstimator::rotateQuaternionByHeading(geometry_msgs::Quaternion_<std::allocator<void> > const&, double) const428536
+
+
+ + + +
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..7134183f79 --- /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-12-17 22:35:29Functions: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      195352 : std::optional<mrs_msgs::UavState> StateEstimator::getUavState() {
+       9             : 
+      10      195352 :   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      390601 :   return mrs_lib::get_mutexed(mtx_uav_state_, uav_state_);
+      16             : }
+      17             : /*//}*/
+      18             : 
+      19             : /*//{ getInnovation() */
+      20      176277 : nav_msgs::Odometry StateEstimator::getInnovation() const {
+      21      176277 :   return mrs_lib::get_mutexed(mtx_innovation_, innovation_);
+      22             : }
+      23             : /*//}*/
+      24             : 
+      25             : /*//{ getPoseCovariance() */
+      26      176277 : std::vector<double> StateEstimator::getPoseCovariance() const {
+      27      176277 :   return mrs_lib::get_mutexed(mtx_covariance_, pose_covariance_.values);
+      28             : }
+      29             : /*//}*/
+      30             : 
+      31             : /*//{ getTwistCovariance() */
+      32      176277 : std::vector<double> StateEstimator::getTwistCovariance() const {
+      33      176277 :   return mrs_lib::get_mutexed(mtx_covariance_, twist_covariance_.values);
+      34             : }
+      35             : /*//}*/
+      36             : 
+      37             : /*//{ publishUavState() */
+      38      211552 : void StateEstimator::publishUavState() const {
+      39             : 
+      40      211552 :   if (!ch_->debug_topics.state) {
+      41           0 :     return;
+      42             :   }
+      43             : 
+      44      423005 :   auto uav_state = mrs_lib::get_mutexed(mtx_uav_state_, uav_state_);
+      45      211512 :   ph_uav_state_.publish(uav_state);
+      46             : }
+      47             : /*//}*/
+      48             : 
+      49             : /*//{ publishOdom() */
+      50      211556 : void StateEstimator::publishOdom() const {
+      51             : 
+      52      423112 :   auto odom = mrs_lib::get_mutexed(mtx_odom_, odom_);
+      53      211555 :   ph_odom_.publish(odom);
+      54      211556 : }
+      55             : /*//}*/
+      56             : 
+      57             : /*//{ publishCovariance() */
+      58      211556 : void StateEstimator::publishCovariance() const {
+      59             : 
+      60      211556 :   if (!ch_->debug_topics.covariance) {
+      61      211556 :     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      211556 : void StateEstimator::publishInnovation() const {
+      73             : 
+      74      211556 :   if (!ch_->debug_topics.innovation) {
+      75      211556 :     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      428536 : std::optional<geometry_msgs::Quaternion> StateEstimator::rotateQuaternionByHeading(const geometry_msgs::Quaternion& q, const double hdg) const {
+      85             : 
+      86             :   try {
+      87      428536 :     tf2::Quaternion tf2_q = mrs_lib::AttitudeConverter(q);
+      88             : 
+      89             :     // Obtain heading from quaternion
+      90      427508 :     double q_hdg = 0;
+      91      427508 :     q_hdg        = mrs_lib::AttitudeConverter(q).getHeading();
+      92             : 
+      93             :     // Build rotation matrix from difference between new heading and quaternion heading
+      94      428137 :     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      425790 :     geometry_msgs::Quaternion q_new = mrs_lib::AttitudeConverter(tf2::Transform(rot_mat) * tf2_q);
+      98      424546 :     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         117 : bool StateEstimator::isCompatibleWithHwApi(const mrs_msgs::HwApiCapabilitiesConstPtr& hw_api_capabilities) const {
+     109             : 
+     110         117 :   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         117 :   ph_->param_loader->loadParam("requires/gnss", requires_gnss);
+     116         117 :   ph_->param_loader->loadParam("requires/imu", requires_imu);
+     117         117 :   ph_->param_loader->loadParam("requires/distance_sensor", requires_distance_sensor);
+     118         117 :   ph_->param_loader->loadParam("requires/altitude", requires_altitude);
+     119         117 :   ph_->param_loader->loadParam("requires/magnetometer_heading", requires_magnetometer_heading);
+     120         117 :   ph_->param_loader->loadParam("requires/position", requires_position);
+     121         117 :   ph_->param_loader->loadParam("requires/orientation", requires_orientation);
+     122         117 :   ph_->param_loader->loadParam("requires/velocity", requires_velocity);
+     123         117 :   ph_->param_loader->loadParam("requires/angular_velocity", requires_angular_velocity);
+     124             : 
+     125         117 :   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         117 :   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         117 :   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         117 :   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         117 :   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         117 :   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         117 :   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         117 :   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         117 :   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         117 :   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         117 :   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..042b5a9dde --- /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:43866665.8 %
Date:2024-12-17 22:35:29Functions:404883.3 %
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 +
71.6%71.6%
+ 		71.6 %63 / 8882.6 %19 / 23
<unnamed>71.6 %63 / 8882.6 %19 / 23
estimation_manager.cpp +
64.9%64.9%
+ 		64.9 %375 / 57884.0 %21 / 25
<unnamed>64.9 %375 / 57884.0 %21 / 25
+
+
+ + + + +
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..f2166fb89e --- /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:43866665.8 %
Date:2024-12-17 22:35:29Functions:404883.3 %
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 +
64.9%64.9%
+ 		64.9 %375 / 57884.0 %21 / 25
<unnamed>64.9 %375 / 57884.0 %21 / 25
estimator.cpp +
71.6%71.6%
+ 		71.6 %63 / 8882.6 %19 / 23
<unnamed>71.6 %63 / 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..39b4ab28a2 --- /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:43866665.8 %
Date:2024-12-17 22:35:29Functions:404883.3 %
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 +
64.9%64.9%
+ 		64.9 %375 / 57884.0 %21 / 25
<unnamed>64.9 %375 / 57884.0 %21 / 25
estimator.cpp +
71.6%71.6%
+ 		71.6 %63 / 8882.6 %19 / 23
<unnamed>71.6 %63 / 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..70a76497a5 --- /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:43866665.8 %
Date:2024-12-17 22:35:29Functions:404883.3 %
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 +
71.6%71.6%
+ 		71.6 %63 / 8882.6 %19 / 23
estimation_manager.cpp +
64.9%64.9%
+ 		64.9 %375 / 57884.0 %21 / 25
+
+
+ + + + +
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..0f8a1b8b39 --- /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:43866665.8 %
Date:2024-12-17 22:35:29Functions:404883.3 %
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 +
64.9%64.9%
+ 		64.9 %375 / 57884.0 %21 / 25
estimator.cpp +
71.6%71.6%
+ 		71.6 %63 / 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..455f95768f --- /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:43866665.8 %
Date:2024-12-17 22:35:29Functions:404883.3 %
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 +
64.9%64.9%
+ 		64.9 %375 / 57884.0 %21 / 25
estimator.cpp +
71.6%71.6%
+ 		71.6 %63 / 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..c2f344e2c8 --- /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-12-17 22:35:29Functions: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()109
mrs_uav_managers::gain_manager::GainManager::onInit()109
mrs_uav_managers::gain_manager::GainManager::setGains(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)110
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&)2406
mrs_uav_managers::gain_manager::GainManager::timerDiagnostics(ros::TimerEvent const&)2422
mrs_uav_managers::gain_manager::GainManager::timerGainManagement(ros::TimerEvent const&)24755
+
+
+ + + +
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..58c8a388a0 --- /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-12-17 22:35:29Functions:77100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()109
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&)2406
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&)2422
mrs_uav_managers::gain_manager::GainManager::timerGainManagement(ros::TimerEvent const&)24755
mrs_uav_managers::gain_manager::GainManager::onInit()109
mrs_uav_managers::gain_manager::GainManager::setGains(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)110
+
+
+ + + +
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..dea934168e --- /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-12-17 22:35:29Functions: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         109 : void GainManager::onInit() {
+     123             : 
+     124         218 :   ros::NodeHandle nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     125             : 
+     126         109 :   ros::Time::waitForValid();
+     127             : 
+     128         109 :   ROS_INFO("[GainManager]: initializing");
+     129             : 
+     130             :   // | ------------------------- params ------------------------- |
+     131             : 
+     132         218 :   mrs_lib::ParamLoader param_loader(nh_, "GainManager");
+     133             : 
+     134         218 :   std::string custom_config_path;
+     135         218 :   std::string platform_config_path;
+     136             : 
+     137         109 :   param_loader.loadParam("custom_config", custom_config_path);
+     138         109 :   param_loader.loadParam("platform_config", platform_config_path);
+     139             : 
+     140         109 :   if (custom_config_path != "") {
+     141         109 :     param_loader.addYamlFile(custom_config_path);
+     142             :   }
+     143             : 
+     144         109 :   if (platform_config_path != "") {
+     145         109 :     param_loader.addYamlFile(platform_config_path);
+     146             :   }
+     147             : 
+     148         109 :   param_loader.addYamlFileFromParam("private_config");
+     149         109 :   param_loader.addYamlFileFromParam("public_config");
+     150         109 :   param_loader.addYamlFileFromParam("public_gains");
+     151             : 
+     152         218 :   const std::string yaml_prefix = "mrs_uav_managers/gain_manager/";
+     153             : 
+     154             :   // params passed from the launch file are not prefixed
+     155         109 :   param_loader.loadParam("enable_profiler", _profiler_enabled_);
+     156             : 
+     157         109 :   param_loader.loadParam(yaml_prefix + "gains", _gain_names_);
+     158             : 
+     159         109 :   param_loader.loadParam(yaml_prefix + "estimator_types", _current_state_estimators_);
+     160             : 
+     161         109 :   param_loader.loadParam(yaml_prefix + "rate", _gain_management_rate_);
+     162         109 :   param_loader.loadParam(yaml_prefix + "diagnostics_rate", _diagnostics_rate_);
+     163             : 
+     164             :   // | ------------------- scope timer logger ------------------- |
+     165             : 
+     166         109 :   param_loader.loadParam(yaml_prefix + "scope_timer/enabled", scope_timer_enabled_);
+     167         327 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+     168         109 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+     169             : 
+     170         109 :   std::vector<std::string>::iterator it;
+     171             : 
+     172             :   // loading gain_names
+     173         327 :   for (it = _gain_names_.begin(); it != _gain_names_.end(); ++it) {
+     174             : 
+     175         218 :     ROS_INFO_STREAM("[GainManager]: loading gains '" << *it << "'");
+     176             : 
+     177         218 :     Gains_t new_gains;
+     178             : 
+     179         218 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kp", new_gains.kpxy);
+     180         218 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kv", new_gains.kvxy);
+     181         218 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/ka", new_gains.kaxy);
+     182         218 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kib", new_gains.kibxy);
+     183         218 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kiw", new_gains.kiwxy);
+     184         218 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kib_lim", new_gains.kibxy_lim);
+     185         218 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kiw_lim", new_gains.kiwxy_lim);
+     186             : 
+     187         218 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/kp", new_gains.kpz);
+     188         218 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/kv", new_gains.kvz);
+     189         218 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ka", new_gains.kaz);
+     190             : 
+     191         218 :     param_loader.loadParam(yaml_prefix + *it + "/attitude/kq_roll_pitch", new_gains.kqrp);
+     192         218 :     param_loader.loadParam(yaml_prefix + *it + "/attitude/kq_yaw", new_gains.kqy);
+     193             : 
+     194         218 :     param_loader.loadParam(yaml_prefix + *it + "/mass_estimator/km", new_gains.km);
+     195         218 :     param_loader.loadParam(yaml_prefix + *it + "/mass_estimator/km_lim", new_gains.km_lim);
+     196             : 
+     197         218 :     _gains_.insert(std::pair<std::string, Gains_t>(*it, new_gains));
+     198             :   }
+     199             : 
+     200             :   // loading the allowed gains lists
+     201         872 :   for (it = _current_state_estimators_.begin(); it != _current_state_estimators_.end(); ++it) {
+     202             : 
+     203         763 :     std::vector<std::string> temp_vector;
+     204         763 :     param_loader.loadParam(yaml_prefix + "allowed_gains/" + *it, temp_vector);
+     205             : 
+     206         763 :     std::vector<std::string>::iterator it2;
+     207        2289 :     for (it2 = temp_vector.begin(); it2 != temp_vector.end(); ++it2) {
+     208        1526 :       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         763 :     _map_type_allowed_gains_.insert(std::pair<std::string, std::vector<std::string>>(*it, temp_vector));
+     215             :   }
+     216             : 
+     217             :   // loading the default gains
+     218         872 :   for (it = _current_state_estimators_.begin(); it != _current_state_estimators_.end(); ++it) {
+     219             : 
+     220         763 :     std::string temp_str;
+     221         763 :     param_loader.loadParam(yaml_prefix + "default_gains/" + *it, temp_str);
+     222             : 
+     223         763 :     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         763 :     _map_type_default_gains_.insert(std::pair<std::string, std::string>(*it, temp_str));
+     229             :   }
+     230             : 
+     231         109 :   ROS_INFO("[GainManager]: done loading dynamical params");
+     232             : 
+     233         109 :   current_gains_       = "";
+     234         109 :   last_estimator_name_ = "";
+     235             : 
+     236             :   // | ------------------------ services ------------------------ |
+     237             : 
+     238         109 :   service_server_set_gains_ = nh_.advertiseService("set_gains_in", &GainManager::callbackSetGains, this);
+     239             : 
+     240         109 :   service_client_set_gains_ = nh_.serviceClient<dynamic_reconfigure::Reconfigure>("set_gains_out");
+     241             : 
+     242             :   // | ----------------------- subscribers ---------------------- |
+     243             : 
+     244         218 :   mrs_lib::SubscribeHandlerOptions shopts;
+     245         109 :   shopts.nh                 = nh_;
+     246         109 :   shopts.node_name          = "GainManager";
+     247         109 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     248         109 :   shopts.threadsafe         = true;
+     249         109 :   shopts.autostart          = true;
+     250         109 :   shopts.queue_size         = 10;
+     251         109 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     252             : 
+     253         109 :   sh_estimation_diag_      = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, "estimation_diagnostics_in");
+     254         109 :   sh_control_manager_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>(shopts, "control_manager_diagnostics_in");
+     255             : 
+     256             :   // | ----------------------- publishers ----------------------- |
+     257             : 
+     258         109 :   ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics>(nh_, "diagnostics_out", 10);
+     259             : 
+     260             :   // | ------------------------- timers ------------------------- |
+     261             : 
+     262         109 :   timer_gain_management_ = nh_.createTimer(ros::Rate(_gain_management_rate_), &GainManager::timerGainManagement, this);
+     263         109 :   timer_diagnostics_     = nh_.createTimer(ros::Rate(_diagnostics_rate_), &GainManager::timerDiagnostics, this);
+     264             : 
+     265             :   // | ------------------------ profiler ------------------------ |
+     266             : 
+     267         109 :   profiler_ = mrs_lib::Profiler(nh_, "GainManager", _profiler_enabled_);
+     268             : 
+     269             :   // | ----------------------- finish init ---------------------- |
+     270             : 
+     271         109 :   if (!param_loader.loadedSuccessfully()) {
+     272           0 :     ROS_ERROR("[GainManager]: could not load all parameters!");
+     273           0 :     ros::shutdown();
+     274             :   }
+     275             : 
+     276         109 :   is_initialized_ = true;
+     277             : 
+     278         109 :   ROS_INFO("[GainManager]: initialized");
+     279             : 
+     280         109 :   ROS_DEBUG("[GainManager]: debug output is enabled");
+     281         109 : }
+     282             : 
+     283             : //}
+     284             : 
+     285             : // --------------------------------------------------------------
+     286             : // |                           methods                          |
+     287             : // --------------------------------------------------------------
+     288             : 
+     289             : /* setGains() //{ */
+     290             : 
+     291         110 : bool GainManager::setGains(std::string gains_name) {
+     292             : 
+     293         110 :   std::map<std::string, Gains_t>::iterator it;
+     294         110 :   it = _gains_.find(gains_name);
+     295             : 
+     296         110 :   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         220 :   dynamic_reconfigure::Config          conf;
+     302         220 :   dynamic_reconfigure::DoubleParameter param;
+     303             : 
+     304         110 :   param.name  = "kpxy";
+     305         110 :   param.value = it->second.kpxy;
+     306         110 :   conf.doubles.push_back(param);
+     307             : 
+     308         110 :   param.name  = "kvxy";
+     309         110 :   param.value = it->second.kvxy;
+     310         110 :   conf.doubles.push_back(param);
+     311             : 
+     312         110 :   param.name  = "kaxy";
+     313         110 :   param.value = it->second.kaxy;
+     314         110 :   conf.doubles.push_back(param);
+     315             : 
+     316         110 :   param.name  = "kq_roll_pitch";
+     317         110 :   param.value = it->second.kqrp;
+     318         110 :   conf.doubles.push_back(param);
+     319             : 
+     320         110 :   param.name  = "kibxy";
+     321         110 :   param.value = it->second.kibxy;
+     322         110 :   conf.doubles.push_back(param);
+     323             : 
+     324         110 :   param.name  = "kiwxy";
+     325         110 :   param.value = it->second.kiwxy;
+     326         110 :   conf.doubles.push_back(param);
+     327             : 
+     328         110 :   param.name  = "kibxy_lim";
+     329         110 :   param.value = it->second.kibxy_lim;
+     330         110 :   conf.doubles.push_back(param);
+     331             : 
+     332         110 :   param.name  = "kiwxy_lim";
+     333         110 :   param.value = it->second.kiwxy_lim;
+     334         110 :   conf.doubles.push_back(param);
+     335             : 
+     336         110 :   param.name  = "kpz";
+     337         110 :   param.value = it->second.kpz;
+     338         110 :   conf.doubles.push_back(param);
+     339             : 
+     340         110 :   param.name  = "kvz";
+     341         110 :   param.value = it->second.kvz;
+     342         110 :   conf.doubles.push_back(param);
+     343             : 
+     344         110 :   param.name  = "kaz";
+     345         110 :   param.value = it->second.kaz;
+     346         110 :   conf.doubles.push_back(param);
+     347             : 
+     348         110 :   param.name  = "kq_yaw";
+     349         110 :   param.value = it->second.kqy;
+     350         110 :   conf.doubles.push_back(param);
+     351             : 
+     352         110 :   param.name  = "km";
+     353         110 :   param.value = it->second.km;
+     354         110 :   conf.doubles.push_back(param);
+     355             : 
+     356         110 :   param.name  = "km_lim";
+     357         110 :   param.value = it->second.km_lim;
+     358         110 :   conf.doubles.push_back(param);
+     359             : 
+     360         220 :   dynamic_reconfigure::ReconfigureRequest  srv_req;
+     361         220 :   dynamic_reconfigure::ReconfigureResponse srv_resp;
+     362             : 
+     363         110 :   srv_req.config = conf;
+     364             : 
+     365         220 :   dynamic_reconfigure::Reconfigure reconf;
+     366         110 :   reconf.request = srv_req;
+     367             : 
+     368         110 :   ROS_INFO_THROTTLE(1.0, "[GainManager]: setting up gains for '%s'", gains_name.c_str());
+     369             : 
+     370         110 :   bool res = service_client_set_gains_.call(reconf);
+     371             : 
+     372         110 :   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         110 :     mrs_lib::set_mutexed(mutex_current_gains_, gains_name, current_gains_);
+     380             : 
+     381         110 :     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       24755 : void GainManager::timerGainManagement(const ros::TimerEvent& event) {
+     470             : 
+     471       24755 :   if (!is_initialized_) {
+     472        5730 :     return;
+     473             :   }
+     474             : 
+     475       49510 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("gainManagementTimer", _gain_management_rate_, 0.01, event);
+     476       49510 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("GainManager::gainManagementTimer", scope_timer_logger_, scope_timer_enabled_);
+     477             : 
+     478       24755 :   if (!sh_estimation_diag_.hasMsg()) {
+     479        5730 :     return;
+     480             :   }
+     481             : 
+     482       19025 :   if (!sh_control_manager_diag_.hasMsg()) {
+     483           0 :     return;
+     484             :   }
+     485             : 
+     486       38050 :   auto estimation_diagnostics = sh_estimation_diag_.getMsg();
+     487             : 
+     488       38050 :   auto control_manager_diagnostics = sh_estimation_diag_.getMsg();
+     489             : 
+     490       38050 :   auto current_gains       = mrs_lib::get_mutexed(mutex_current_gains_, current_gains_);
+     491       19025 :   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       19025 :   if (estimation_diagnostics->current_state_estimator != last_estimator_name) {
+     495             : 
+     496         114 :     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         114 :     std::map<std::string, std::string>::iterator it;
+     500         114 :     it = _map_type_default_gains_.find(estimation_diagnostics->current_state_estimator);
+     501             : 
+     502         114 :     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         114 :       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         109 :         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         109 :         if (setGains(it->second)) {
+     521             : 
+     522         109 :           last_estimator_name = estimation_diagnostics->current_state_estimator;
+     523             : 
+     524         109 :           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       19025 :   mrs_lib::set_mutexed(mutex_last_estimator_name_, last_estimator_name, last_estimator_name_);
+     535             : }
+     536             : 
+     537             : //}
+     538             : 
+     539             : /* timerDiagnostics() //{ */
+     540             : 
+     541        2422 : void GainManager::timerDiagnostics(const ros::TimerEvent& event) {
+     542             : 
+     543        2422 :   if (!is_initialized_) {
+     544         560 :     return;
+     545             :   }
+     546             : 
+     547        4844 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerDiagnostics", _diagnostics_rate_, 0.01, event);
+     548        4844 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("GainManager::timerDiagnostics", scope_timer_logger_, scope_timer_enabled_);
+     549             : 
+     550        2422 :   if (!sh_estimation_diag_.hasMsg()) {
+     551         556 :     ROS_WARN_THROTTLE(10.0, "[GainManager]: can not do gain management, missing estimator diagnostics!");
+     552         556 :     return;
+     553             :   }
+     554             : 
+     555        1866 :   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        1866 :   auto current_gains = mrs_lib::get_mutexed(mutex_current_gains_, current_gains_);
+     561             : 
+     562        1866 :   if (current_gains == "") { // this could happend just before timerGainManagement() finishes
+     563           4 :     return;
+     564             :   }
+     565             : 
+     566        1862 :   auto estimation_diagnostics = sh_estimation_diag_.getMsg();
+     567             : 
+     568        1862 :   mrs_msgs::GainManagerDiagnostics diagnostics;
+     569             : 
+     570        1862 :   diagnostics.stamp        = ros::Time::now();
+     571        1862 :   diagnostics.current_name = current_gains;
+     572        1862 :   diagnostics.loaded       = _gain_names_;
+     573             : 
+     574             :   // get the available gains
+     575             :   {
+     576        1862 :     std::map<std::string, std::vector<std::string>>::iterator it;
+     577        1862 :     it = _map_type_allowed_gains_.find(estimation_diagnostics->current_state_estimator);
+     578             : 
+     579        1862 :     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        1862 :       diagnostics.available = it->second;
+     584             :     }
+     585             :   }
+     586             : 
+     587             :   // get the current gain values
+     588             :   {
+     589        1862 :     std::map<std::string, Gains_t>::iterator it;
+     590        1862 :     it = _gains_.find(current_gains);
+     591             : 
+     592        1862 :     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        1862 :     diagnostics.current_values.kpxy = it->second.kpxy;
+     598        1862 :     diagnostics.current_values.kvxy = it->second.kvxy;
+     599        1862 :     diagnostics.current_values.kaxy = it->second.kaxy;
+     600             : 
+     601        1862 :     diagnostics.current_values.kqrp = it->second.kqrp;
+     602             : 
+     603        1862 :     diagnostics.current_values.kibxy     = it->second.kibxy;
+     604        1862 :     diagnostics.current_values.kibxy_lim = it->second.kibxy_lim;
+     605             : 
+     606        1862 :     diagnostics.current_values.kiwxy     = it->second.kiwxy;
+     607        1862 :     diagnostics.current_values.kiwxy_lim = it->second.kiwxy_lim;
+     608             : 
+     609        1862 :     diagnostics.current_values.kpz = it->second.kpz;
+     610        1862 :     diagnostics.current_values.kvz = it->second.kvz;
+     611        1862 :     diagnostics.current_values.kaz = it->second.kaz;
+     612             : 
+     613        1862 :     diagnostics.current_values.kqy = it->second.kqy;
+     614             : 
+     615        1862 :     diagnostics.current_values.km     = it->second.km;
+     616        1862 :     diagnostics.current_values.km_lim = it->second.km_lim;
+     617             :   }
+     618             : 
+     619        1862 :   ph_diagnostics_.publish(diagnostics);
+     620             : }
+     621             : 
+     622             : //}
+     623             : 
+     624             : // --------------------------------------------------------------
+     625             : // |                          routines                          |
+     626             : // --------------------------------------------------------------
+     627             : 
+     628             : /* stringInVector() //{ */
+     629             : 
+     630        2406 : bool GainManager::stringInVector(const std::string& value, const std::vector<std::string>& vector) {
+     631             : 
+     632        2406 :   if (std::find(vector.begin(), vector.end(), value) == vector.end()) {
+     633         110 :     return false;
+     634             :   } else {
+     635        2296 :     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         109 : 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:1252172972.4 %
Date:2024-12-17 22:35:29Functions: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 +
67.1%67.1%
+ 		67.1 %792 / 1180100.0 %38 / 38
<unnamed>67.1 %792 / 1180100.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..98f784d118 --- /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:1252172972.4 %
Date:2024-12-17 22:35:29Functions: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 +
67.1%67.1%
+ 		67.1 %792 / 1180100.0 %38 / 38
<unnamed>67.1 %792 / 1180100.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..35b086eae9 --- /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:1252172972.4 %
Date:2024-12-17 22:35:29Functions: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 +
67.1%67.1%
+ 		67.1 %792 / 1180100.0 %38 / 38
<unnamed>67.1 %792 / 1180100.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..d5ca0f2edb --- /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:1252172972.4 %
Date:2024-12-17 22:35:29Functions: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 +
67.1%67.1%
+ 		67.1 %792 / 1180100.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..f0ccb33bd5 --- /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:1252172972.4 %
Date:2024-12-17 22:35:29Functions: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 +
67.1%67.1%
+ 		67.1 %792 / 1180100.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..3a571c65fb --- /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:1252172972.4 %
Date:2024-12-17 22:35:29Functions: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 +
67.1%67.1%
+ 		67.1 %792 / 1180100.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..64d0b4a37d --- /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-12-17 22:35:29Functions: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()109
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>)109
mrs_uav_managers::NullTracker::~NullTracker()109
mrs_uav_managers::NullTracker::~NullTracker().2109
mrs_uav_managers::NullTracker::deactivate()241
mrs_uav_managers::NullTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)245
mrs_uav_managers::NullTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)312
mrs_uav_managers::NullTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)387
mrs_uav_managers::NullTracker::getStatus()7572
mrs_uav_managers::NullTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)142109
+
+
+ + + +
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..cbf4b2a5e6 --- /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-12-17 22:35:29Functions:112055.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()109
mrs_uav_managers::NullTracker::deactivate()241
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>)109
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&)387
mrs_uav_managers::NullTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)312
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&)142109
mrs_uav_managers::NullTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)245
mrs_uav_managers::NullTracker::getStatus()7572
mrs_uav_managers::NullTracker::~NullTracker()109
mrs_uav_managers::NullTracker::~NullTracker().2109
+
+
+ + + +
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..883bfe55d4 --- /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-12-17 22:35:29Functions: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         218 :   ~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         109 : 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         218 :   ros::NodeHandle nh_(parent_nh, "null_tracker");
+      59             : 
+      60         109 :   ros::Time::waitForValid();
+      61             : 
+      62         109 :   is_initialized = true;
+      63             : 
+      64         109 :   this->common_handlers = common_handlers;
+      65             : 
+      66         109 :   ROS_INFO("[NullTracker]: initialized");
+      67             : 
+      68         218 :   return true;
+      69             : }
+      70             : 
+      71             : //}
+      72             : 
+      73             : /* //{ activate() */
+      74             : 
+      75         245 : std::tuple<bool, std::string> NullTracker::activate([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd) {
+      76             : 
+      77         245 :   std::stringstream ss;
+      78         245 :   ss << "activated";
+      79             : 
+      80         245 :   ROS_INFO_STREAM("[NullTracker]: " << ss.str());
+      81         245 :   is_active = true;
+      82             : 
+      83         490 :   return std::tuple(true, ss.str());
+      84             : }
+      85             : 
+      86             : //}
+      87             : 
+      88             : /* //{ deactivate() */
+      89             : 
+      90         241 : void NullTracker::deactivate(void) {
+      91             : 
+      92         241 :   ROS_INFO("[NullTracker]: deactivated");
+      93         241 :   is_active = false;
+      94         241 : }
+      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      142109 : 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      142109 :   return {};
+     120             : }
+     121             : 
+     122             : //}
+     123             : 
+     124             : /* //{ getStatus() */
+     125             : 
+     126        7572 : const mrs_msgs::TrackerStatus NullTracker::getStatus() {
+     127             : 
+     128        7572 :   mrs_msgs::TrackerStatus tracker_status;
+     129             : 
+     130        7572 :   tracker_status.active            = is_active;
+     131        7572 :   tracker_status.callbacks_enabled = callbacks_enabled;
+     132             : 
+     133        7572 :   return tracker_status;
+     134             : }
+     135             : 
+     136             : //}
+     137             : 
+     138             : /* //{ enableCallbacks() */
+     139             : 
+     140         312 : const std_srvs::SetBoolResponse::ConstPtr NullTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     141             : 
+     142         624 :   std_srvs::SetBoolResponse res;
+     143             : 
+     144         312 :   std::stringstream ss;
+     145             : 
+     146         312 :   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         299 :     ss << "callbacks were already " << (callbacks_enabled ? "enabled" : "disabled");
+     157             :   }
+     158             : 
+     159         312 :   res.message = ss.str();
+     160         312 :   res.success = true;
+     161             : 
+     162         936 :   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         387 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr NullTracker::setConstraints([
+     238             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     239             : 
+     240         387 :   return mrs_msgs::DynamicsConstraintsSrvResponse::Ptr();
+     241             : }
+     242             : 
+     243             : //}
+     244             : 
+     245             : }  // namespace mrs_uav_managers
+     246             : 
+     247             : #include <pluginlib/class_list_macros.h>
+     248         109 : 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:35143181.4 %
Date:2024-12-17 22:35:29Functions: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 %351 / 43192.9 %13 / 14
<unnamed>81.4 %351 / 43192.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..4ae4f04cee --- /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:35143181.4 %
Date:2024-12-17 22:35:29Functions: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 %351 / 43192.9 %13 / 14
<unnamed>81.4 %351 / 43192.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..1f3b3c1c87 --- /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:35143181.4 %
Date:2024-12-17 22:35:29Functions: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 %351 / 43192.9 %13 / 14
<unnamed>81.4 %351 / 43192.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..541cf12d8a --- /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:35143181.4 %
Date:2024-12-17 22:35:29Functions: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 %351 / 43192.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..82f66d1334 --- /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:35143181.4 %
Date:2024-12-17 22:35:29Functions: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 %351 / 43192.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..c98cff502d --- /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:35143181.4 %
Date:2024-12-17 22:35:29Functions: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 %351 / 43192.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..9a204986e0 --- /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:35143181.4 %
Date:2024-12-17 22:35:29Functions: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()109
mrs_uav_managers::transform_manager::TransformManager::publishLocalTf()109
mrs_uav_managers::transform_manager::TransformManager::onInit()109
mrs_uav_managers::transform_manager::TransformManager::TransformManager()109
mrs_uav_managers::transform_manager::TransformManager::getPrintName[abi:cxx11]() const1756
mrs_uav_managers::transform_manager::TransformManager::callbackRtkGps(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)2732
mrs_uav_managers::transform_manager::TransformManager::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)82187
mrs_uav_managers::transform_manager::TransformManager::callbackAltitudeAmsl(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>)106973
mrs_uav_managers::transform_manager::TransformManager::callbackHeightAgl(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>)131603
mrs_uav_managers::transform_manager::TransformManager::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)176199
mrs_uav_managers::transform_manager::TransformManager::publishFcuUntiltedTf(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)235808
mrs_uav_managers::transform_manager::TransformManager::callbackHwApiOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)235808
+
+
+ + + +
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..508dba2072 --- /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:35143181.4 %
Date:2024-12-17 22:35:29Functions:131492.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()109
mrs_uav_managers::transform_manager::TransformManager::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)82187
mrs_uav_managers::transform_manager::TransformManager::callbackRtkGps(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)2732
mrs_uav_managers::transform_manager::TransformManager::publishLocalTf()109
mrs_uav_managers::transform_manager::TransformManager::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)176199
mrs_uav_managers::transform_manager::TransformManager::callbackHeightAgl(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>)131603
mrs_uav_managers::transform_manager::TransformManager::callbackAltitudeAmsl(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>)106973
mrs_uav_managers::transform_manager::TransformManager::publishFcuUntiltedTf(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)235808
mrs_uav_managers::transform_manager::TransformManager::callbackHwApiOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)235808
mrs_uav_managers::transform_manager::TransformManager::onInit()109
mrs_uav_managers::transform_manager::TransformManager::TransformManager()109
mrs_uav_managers::transform_manager::TransformManager::getPrintName[abi:cxx11]() const1756
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..e2124fc504 --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.html @@ -0,0 +1,1041 @@ + + + + + + + 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:35143181.4 %
Date:2024-12-17 22:35:29Functions: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         109 :   TransformManager() {
+      50         109 :     ch_ = std::make_shared<estimation_manager::CommonHandlers_t>();
+      51             : 
+      52         109 :     ch_->nodelet_name = nodelet_name_;
+      53         109 :     ch_->package_name = package_name_;
+      54         109 :   }
+      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         109 : void TransformManager::onInit() {
+     150             : 
+     151         109 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     152             : 
+     153         109 :   ros::Time::waitForValid();
+     154             : 
+     155         109 :   ROS_INFO("[%s]: initializing", getPrintName().c_str());
+     156             : 
+     157         109 :   broadcaster_ = std::make_shared<mrs_lib::TransformBroadcaster>();
+     158             : 
+     159         109 :   ch_->transformer = std::make_shared<mrs_lib::Transformer>(nh_, getPrintName());
+     160         109 :   ch_->transformer->retryLookupNewest(true);
+     161             : 
+     162         218 :   mrs_lib::ParamLoader param_loader(nh_, getPrintName());
+     163             : 
+     164         109 :   param_loader.loadParam("custom_config", _custom_config_);
+     165         109 :   param_loader.loadParam("platform_config", _platform_config_);
+     166         109 :   param_loader.loadParam("world_config", _world_config_);
+     167             : 
+     168         109 :   if (_custom_config_ != "") {
+     169         109 :     param_loader.addYamlFile(_custom_config_);
+     170             :   }
+     171             : 
+     172         109 :   if (_platform_config_ != "") {
+     173         109 :     param_loader.addYamlFile(_platform_config_);
+     174             :   }
+     175             : 
+     176         109 :   if (_world_config_ != "") {
+     177         109 :     param_loader.addYamlFile(_world_config_);
+     178             :   }
+     179             : 
+     180         109 :   param_loader.addYamlFileFromParam("private_config");
+     181         109 :   param_loader.addYamlFileFromParam("public_config");
+     182         109 :   param_loader.addYamlFileFromParam("estimators_config");
+     183             : 
+     184         218 :   const std::string yaml_prefix = "mrs_uav_managers/transform_manager/";
+     185             : 
+     186         109 :   param_loader.loadParam("uav_name", ch_->uav_name);
+     187             : 
+     188             :   /*//{ initialize scope timer */
+     189         109 :   param_loader.loadParam(yaml_prefix + "scope_timer/enabled", ch_->scope_timer.enabled);
+     190         218 :   std::string       filepath;
+     191         218 :   const std::string time_logger_filepath = ros::package::getPath(package_name_) + "/scope_timer/transform_manager_scope_timer.txt";
+     192         109 :   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         109 :   bool   is_origin_param_ok = true;
+     198         109 :   double world_origin_x     = 0;
+     199         109 :   double world_origin_y     = 0;
+     200             : 
+     201         109 :   param_loader.loadParam("world_origin/units", world_origin_units_);
+     202             : 
+     203         109 :   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         109 :   } else if (Support::toLowercase(world_origin_units_) == "latlon") {
+     211             : 
+     212         109 :     ROS_INFO("[%s]: Loading world origin in LatLon units.", getPrintName().c_str());
+     213             : 
+     214             :     double lat, lon;
+     215         109 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_x", lat);
+     216         109 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_y", lon);
+     217             : 
+     218         109 :     mrs_lib::UTM(lat, lon, &world_origin_x, &world_origin_y);
+     219             : 
+     220         109 :     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         109 :   world_origin_.x = world_origin_x;
+     228         109 :   world_origin_.y = world_origin_y;
+     229         109 :   world_origin_.z = 0;
+     230             : 
+     231         109 :   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         218 :   std::string local_origin_parent_frame_id;
+     240         109 :   param_loader.loadParam(yaml_prefix + "local_origin_tf/parent", local_origin_parent_frame_id);
+     241         109 :   ns_local_origin_parent_frame_id_ = ch_->uav_name + "/" + local_origin_parent_frame_id;
+     242             : 
+     243         218 :   std::string local_origin_child_frame_id;
+     244         109 :   param_loader.loadParam(yaml_prefix + "local_origin_tf/child", local_origin_child_frame_id);
+     245         109 :   ns_local_origin_child_frame_id_ = ch_->uav_name + "/" + local_origin_child_frame_id;
+     246             : 
+     247         109 :   param_loader.loadParam(yaml_prefix + "local_origin_tf/enabled", publish_local_origin_tf_);
+     248             :   /*//}*/
+     249             : 
+     250             :   /*//{ load stable_origin parameters */
+     251         218 :   std::string stable_origin_parent_frame_id;
+     252         109 :   param_loader.loadParam(yaml_prefix + "stable_origin_tf/parent", stable_origin_parent_frame_id);
+     253         109 :   ns_stable_origin_parent_frame_id_ = ch_->uav_name + "/" + stable_origin_parent_frame_id;
+     254             : 
+     255         218 :   std::string stable_origin_child_frame_id;
+     256         109 :   param_loader.loadParam(yaml_prefix + "stable_origin_tf/child", stable_origin_child_frame_id);
+     257         109 :   ns_stable_origin_child_frame_id_ = ch_->uav_name + "/" + stable_origin_child_frame_id;
+     258             : 
+     259         109 :   param_loader.loadParam(yaml_prefix + "stable_origin_tf/enabled", publish_stable_origin_tf_);
+     260             :   /*//}*/
+     261             : 
+     262             :   /*//{ load fixed_origin parameters */
+     263         218 :   std::string fixed_origin_parent_frame_id;
+     264         109 :   param_loader.loadParam(yaml_prefix + "fixed_origin_tf/parent", fixed_origin_parent_frame_id);
+     265         109 :   ns_fixed_origin_parent_frame_id_ = ch_->uav_name + "/" + fixed_origin_parent_frame_id;
+     266             : 
+     267         218 :   std::string fixed_origin_child_frame_id;
+     268         109 :   param_loader.loadParam(yaml_prefix + "fixed_origin_tf/child", fixed_origin_child_frame_id);
+     269         109 :   ns_fixed_origin_child_frame_id_ = ch_->uav_name + "/" + fixed_origin_child_frame_id;
+     270             : 
+     271         109 :   param_loader.loadParam(yaml_prefix + "fixed_origin_tf/enabled", publish_fixed_origin_tf_);
+     272             :   /*//}*/
+     273             : 
+     274             :   /*//{ load fcu_untilted parameters */
+     275         218 :   std::string fcu_frame_id;
+     276         109 :   param_loader.loadParam(yaml_prefix + "fcu_untilted_tf/parent", fcu_frame_id);
+     277         109 :   ch_->frames.fcu    = fcu_frame_id;
+     278         109 :   ch_->frames.ns_fcu = ch_->uav_name + "/" + fcu_frame_id;
+     279             : 
+     280         218 :   std::string fcu_untilted_frame_id;
+     281         109 :   param_loader.loadParam(yaml_prefix + "fcu_untilted_tf/child", fcu_untilted_frame_id);
+     282         109 :   ch_->frames.fcu_untilted    = fcu_untilted_frame_id;
+     283         109 :   ch_->frames.ns_fcu_untilted = ch_->uav_name + "/" + fcu_untilted_frame_id;
+     284             : 
+     285         109 :   param_loader.loadParam(yaml_prefix + "fcu_untilted_tf/enabled", publish_fcu_untilted_tf_);
+     286             :   /*//}*/
+     287             : 
+     288             :   /*//{ load amsl_origin parameters*/
+     289         218 :   std::string amsl_parent_frame_id, amsl_child_frame_id;
+     290         109 :   param_loader.loadParam(yaml_prefix + "altitude_amsl_tf/enabled", publish_amsl_origin_tf_);
+     291         109 :   param_loader.loadParam(yaml_prefix + "altitude_amsl_tf/parent", amsl_parent_frame_id);
+     292         109 :   param_loader.loadParam(yaml_prefix + "altitude_amsl_tf/child", amsl_child_frame_id);
+     293         109 :   ch_->frames.amsl                = amsl_child_frame_id;
+     294         109 :   ch_->frames.ns_amsl             = ch_->uav_name + "/" + amsl_child_frame_id;
+     295         109 :   ns_amsl_origin_parent_frame_id_ = ch_->uav_name + "/" + amsl_parent_frame_id;
+     296         109 :   ns_amsl_origin_child_frame_id_  = ch_->uav_name + "/" + amsl_child_frame_id;
+     297             : 
+     298             :   /*//}*/
+     299             : 
+     300         109 :   param_loader.loadParam(yaml_prefix + "rtk_antenna/frame_id", ch_->frames.rtk_antenna);
+     301         109 :   ch_->frames.ns_rtk_antenna = ch_->uav_name + "/" + ch_->frames.rtk_antenna;
+     302             : 
+     303         109 :   param_loader.loadParam("mrs_uav_managers/estimation_manager/state_estimators", estimator_names_);
+     304         109 :   param_loader.loadParam(yaml_prefix + "tf_sources", tf_source_names_);
+     305             : 
+     306         109 :   param_loader.loadParam(yaml_prefix + "utm_source_priority", utm_source_priority_list_);
+     307         348 :   for (auto utm_source : utm_source_priority_list_) {
+     308         347 :     if (Support::isStringInVector(utm_source, estimator_names_)) {
+     309         108 :       ROS_INFO("[%s]: the source for utm_origin and world origin is: %s", getPrintName().c_str(), utm_source.c_str());
+     310         108 :       utm_source_name_ = utm_source;
+     311         108 :       break;
+     312             :     }
+     313             :   }
+     314             : 
+     315             :   /*//{ initialize tf sources */
+     316         109 :   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         226 :   for (int i = 0; i < int(estimator_names_.size()); i++) {
+     346             : 
+     347         234 :     const std::string estimator_name = estimator_names_[i];
+     348         117 :     const bool        is_utm_source  = estimator_name == utm_source_name_;
+     349         117 :     ROS_INFO("[%s]: loading tf source of estimator: %s", getPrintName().c_str(), estimator_name.c_str());
+     350             : 
+     351         117 :     auto estimator_param_loader = std::make_shared<mrs_lib::ParamLoader>(nh_, "TransformManager/" + estimator_name);
+     352             : 
+     353         117 :     if (_custom_config_ != "") {
+     354         117 :       estimator_param_loader->addYamlFile(_custom_config_);
+     355             :     }
+     356             : 
+     357         117 :     if (_platform_config_ != "") {
+     358         117 :       estimator_param_loader->addYamlFile(_platform_config_);
+     359             :     }
+     360             : 
+     361         117 :     if (_world_config_ != "") {
+     362         117 :       estimator_param_loader->addYamlFile(_world_config_);
+     363             :     }
+     364             : 
+     365         117 :     estimator_param_loader->addYamlFileFromParam("private_config");
+     366         117 :     estimator_param_loader->addYamlFileFromParam("public_config");
+     367         117 :     estimator_param_loader->addYamlFileFromParam("estimators_config");
+     368             : 
+     369         117 :     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         109 :   param_loader.loadParam(yaml_prefix + "mapping_origin_tf/enabled", mapping_origin_tf_enabled, false);
+     375             : 
+     376         109 :   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         218 :   mrs_lib::SubscribeHandlerOptions shopts;
+     403         109 :   shopts.nh                 = nh_;
+     404         109 :   shopts.node_name          = getPrintName();
+     405         109 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     406         109 :   shopts.threadsafe         = true;
+     407         109 :   shopts.autostart          = true;
+     408         109 :   shopts.queue_size         = 10;
+     409         109 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     410             : 
+     411         109 :   sh_uav_state_ = mrs_lib::SubscribeHandler<mrs_msgs::UavState>(shopts, "uav_state_in", &TransformManager::callbackUavState, this);
+     412             : 
+     413         109 :   sh_height_agl_ = mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>(shopts, "height_agl_in", &TransformManager::callbackHeightAgl, this);
+     414             : 
+     415         109 :   sh_altitude_amsl_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude>(shopts, "altitude_amsl_in", &TransformManager::callbackAltitudeAmsl, this);
+     416             : 
+     417             :   sh_hw_api_orientation_ =
+     418         109 :       mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, "orientation_in", &TransformManager::callbackHwApiOrientation, this);
+     419             : 
+     420         109 :   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         106 :     sh_gnss_ = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, "gnss_in", &TransformManager::callbackGnss, this);
+     424             :   }
+     425             :   /*//}*/
+     426             : 
+     427         109 :   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         109 :   is_initialized_ = true;
+     433         109 :   ROS_INFO("[%s]: initialized", getPrintName().c_str());
+     434         109 : }
+     435             : /*//}*/
+     436             : 
+     437             : /*//{ callbackUavState() */
+     438             : 
+     439      176199 : void TransformManager::callbackUavState(const mrs_msgs::UavState::ConstPtr msg) {
+     440             : 
+     441      176199 :   if (!is_initialized_) {
+     442           0 :     return;
+     443             :   }
+     444             : 
+     445      352398 :   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      176199 :   if (!is_first_frame_id_set_) {
+     449         109 :     first_frame_id_                = msg->header.frame_id;
+     450         109 :     last_frame_id_                 = msg->header.frame_id;
+     451         109 :     pose_fixed_                    = msg->pose;
+     452         109 :     pose_first_.pose               = msg->pose;
+     453         109 :     pose_first_.header             = msg->header;
+     454         109 :     pose_fixed_diff_.orientation.w = 1;
+     455             : 
+     456             :     // we don't want vins_kickoff to be our first estimator
+     457         109 :     if (msg->header.frame_id != ch_->uav_name + "/vins_kickoff_origin") {
+     458         109 :       is_first_frame_id_set_ = true;
+     459             :     }
+     460             :   }
+     461             : 
+     462             :   // publish static tf from fixed_origin to local_origin based on the first message
+     463      176199 :   if (publish_local_origin_tf_ && !is_local_static_tf_published_) {
+     464         109 :     publishLocalTf();
+     465             :   }
+     466             : 
+     467      176199 :   if (publish_stable_origin_tf_) {
+     468             :     /*//{ publish stable_origin tf*/
+     469      176199 :     geometry_msgs::TransformStamped tf_msg;
+     470      176199 :     tf_msg.header.stamp    = msg->header.stamp;
+     471      176199 :     tf_msg.header.frame_id = ns_stable_origin_parent_frame_id_;
+     472      176199 :     tf_msg.child_frame_id  = ns_stable_origin_child_frame_id_;
+     473             : 
+     474             :     // transform pose to first frame_id
+     475      176199 :     geometry_msgs::PoseStamped pose;
+     476      176199 :     pose.header = msg->header;
+     477      176199 :     pose.pose   = msg->pose;
+     478      176199 :     if (pose.pose.orientation.w == 0 && pose.pose.orientation.z == 0 && pose.pose.orientation.y == 0 && pose.pose.orientation.x == 0) {
+     479           0 :       ROS_WARN_ONCE("[%s]: Uninitialized quaternion detected during publishing stable_origin tf of %s. Setting w=1", getPrintName().c_str(),
+     480             :                     pose.header.frame_id.c_str());
+     481           0 :       pose.pose.orientation.w = 1.0;
+     482             :     }
+     483             : 
+     484      176199 :     auto res = ch_->transformer->transformSingle(pose, first_frame_id_);
+     485             : 
+     486      176199 :     if (res) {
+     487      176199 :       const tf2::Transform      tf       = Support::tf2FromPose(res->pose);
+     488      176199 :       const tf2::Transform      tf_inv   = tf.inverse();
+     489      176199 :       const geometry_msgs::Pose pose_inv = Support::poseFromTf2(tf_inv);
+     490      176199 :       tf_msg.transform.translation       = Support::pointToVector3(pose_inv.position);
+     491      176199 :       tf_msg.transform.rotation          = pose_inv.orientation;
+     492             : 
+     493      176199 :       if (Support::noNans(tf_msg)) {
+     494             :         try {
+     495      176199 :           broadcaster_->sendTransform(tf_msg);
+     496             :         }
+     497           0 :         catch (...) {
+     498           0 :           ROS_ERROR("exception caught ");
+     499             :         }
+     500             :       } else {
+     501           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(),
+     502             :                           tf_msg.child_frame_id.c_str());
+     503             :       }
+     504      176199 :       ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     505             :                     tf_msg.child_frame_id.c_str());
+     506             :     } else {
+     507           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());
+     508           0 :       return;
+     509             :     }
+     510             :     /*//}*/
+     511             :   }
+     512             : 
+     513      176199 :   if (publish_fixed_origin_tf_) {
+     514             :     /*//{ publish fixed_origin tf*/
+     515      176199 :     if (msg->header.frame_id != last_frame_id_) {
+     516           5 :       ROS_WARN("[%s]: Detected estimator change from %s to %s. Updating offset for fixed origin.", getPrintName().c_str(), last_frame_id_.c_str(),
+     517             :                msg->header.frame_id.c_str());
+     518             : 
+     519           5 :       pose_fixed_diff_ = Support::getPoseDiff(msg->pose, pose_fixed_);
+     520             :     }
+     521             : 
+     522             : 
+     523      176199 :     pose_fixed_ = Support::applyPoseDiff(msg->pose, pose_fixed_diff_);
+     524             : 
+     525      352398 :     geometry_msgs::TransformStamped tf_msg;
+     526      176199 :     tf_msg.header.stamp    = msg->header.stamp;
+     527      176199 :     tf_msg.header.frame_id = ns_fixed_origin_parent_frame_id_;
+     528      176199 :     tf_msg.child_frame_id  = ns_fixed_origin_child_frame_id_;
+     529             : 
+     530      176199 :     const tf2::Transform      tf       = Support::tf2FromPose(pose_fixed_);
+     531      176199 :     const tf2::Transform      tf_inv   = tf.inverse();
+     532      176199 :     const geometry_msgs::Pose pose_inv = Support::poseFromTf2(tf_inv);
+     533      176199 :     tf_msg.transform.translation       = Support::pointToVector3(pose_inv.position);
+     534      176199 :     tf_msg.transform.rotation          = pose_inv.orientation;
+     535             : 
+     536      176199 :     if (Support::noNans(tf_msg)) {
+     537             :       try {
+     538      176199 :         broadcaster_->sendTransform(tf_msg);
+     539             :       }
+     540           0 :       catch (...) {
+     541           0 :         ROS_ERROR("exception caught ");
+     542             :       }
+     543             :     } else {
+     544           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(),
+     545             :                         tf_msg.child_frame_id.c_str());
+     546             :     }
+     547      176199 :     ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     548             :                   tf_msg.child_frame_id.c_str());
+     549             :     /*//}*/
+     550             :   }
+     551             : 
+     552             :   /*//{ choose another source of utm and world tfs after estimator switch */
+     553      176199 :   if (msg->header.frame_id != last_frame_id_) {
+     554             : 
+     555          10 :     const std::string last_estimator_name    = Support::frameIdToEstimatorName(last_frame_id_);
+     556          10 :     const std::string current_estimator_name = Support::frameIdToEstimatorName(msg->header.frame_id);
+     557             : 
+     558           5 :     ROS_INFO("[%s]: Detected estimator switch: %s -> %s", getPrintName().c_str(), last_estimator_name.c_str(), current_estimator_name.c_str());
+     559             : 
+     560           5 :     bool   valid_utm_source_found = false;
+     561             :     size_t potential_utm_source_index;
+     562             : 
+     563          12 :     for (size_t i = 0; i < tf_sources_.size(); i++) {
+     564             : 
+     565             :       // first check if tf source can publish utm origin and is not the switched-from estimator
+     566          12 :       if (tf_sources_.at(i)->getIsUtmBased() && tf_sources_.at(i)->getName() != last_estimator_name) {
+     567             : 
+     568           8 :         valid_utm_source_found     = true;
+     569           8 :         potential_utm_source_index = i;
+     570             : 
+     571             :         // check if switched-to estimator is utm_based, if so, use it
+     572           8 :         if (tf_sources_.at(i)->getIsUtmBased() && tf_sources_.at(i)->getName() == current_estimator_name) {
+     573           5 :           potential_utm_source_index = i;
+     574           5 :           break;
+     575             :         }
+     576             :       }
+     577             :     }
+     578             : 
+     579             : 
+     580             :     // if we found a valid utm source, use it, otherwise stay with the switched-from estimator
+     581           5 :     if (valid_utm_source_found) {
+     582             : 
+     583             :       // stop all estimators from publishing utm source
+     584          23 :       for (size_t i = 0; i < tf_sources_.size(); i++) {
+     585          18 :         if (tf_sources_.at(i)->getIsUtmSource()) {
+     586           5 :           tf_sources_.at(i)->setIsUtmSource(false);
+     587           5 :           ROS_INFO("[%s]: setting is_utm_source of estimator %s to false", getPrintName().c_str(), last_estimator_name.c_str());
+     588             :         }
+     589             :       }
+     590             : 
+     591           5 :       tf_sources_.at(potential_utm_source_index)->setIsUtmSource(true);
+     592           5 :       ROS_INFO("[%s]: setting is_utm_source of estimator %s to true", getPrintName().c_str(), current_estimator_name.c_str());
+     593             :     }
+     594             :   }
+     595             :   /*//}*/
+     596             : 
+     597      176199 :   last_frame_id_ = msg->header.frame_id;
+     598             : }
+     599             : /*//}*/
+     600             : 
+     601             : /*//{ callbackHeightAgl() */
+     602             : 
+     603      131603 : void TransformManager::callbackHeightAgl(const mrs_msgs::Float64Stamped::ConstPtr msg) {
+     604             : 
+     605      131603 :   if (!is_initialized_) {
+     606           0 :     return;
+     607             :   }
+     608             : 
+     609      394809 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::callbackHeightAgl", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     610             : 
+     611      263206 :   geometry_msgs::TransformStamped tf_msg;
+     612      131603 :   tf_msg.header.stamp    = msg->header.stamp;
+     613      131603 :   tf_msg.header.frame_id = ch_->frames.ns_fcu_untilted;
+     614      131603 :   tf_msg.child_frame_id  = msg->header.frame_id;
+     615             : 
+     616      131603 :   tf_msg.transform.translation.x = 0;
+     617      131603 :   tf_msg.transform.translation.y = 0;
+     618      131603 :   tf_msg.transform.translation.z = -msg->value;
+     619      131603 :   tf_msg.transform.rotation.x    = 0;
+     620      131603 :   tf_msg.transform.rotation.y    = 0;
+     621      131603 :   tf_msg.transform.rotation.z    = 0;
+     622      131603 :   tf_msg.transform.rotation.w    = 1;
+     623             : 
+     624      131603 :   if (Support::noNans(tf_msg)) {
+     625             :     try {
+     626      131603 :       broadcaster_->sendTransform(tf_msg);
+     627             :     }
+     628           0 :     catch (...) {
+     629           0 :       ROS_ERROR("exception caught ");
+     630             :     }
+     631             :   } else {
+     632           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(),
+     633             :                       tf_msg.child_frame_id.c_str());
+     634             :   }
+     635      131603 :   ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     636             :                 tf_msg.child_frame_id.c_str());
+     637             : }
+     638             : /*//}*/
+     639             : 
+     640             : /*//{ callbackAltitudeAmsl() */
+     641             : 
+     642      106973 : void TransformManager::callbackAltitudeAmsl(const mrs_msgs::HwApiAltitude::ConstPtr msg) {
+     643             : 
+     644      106973 :   if (!is_initialized_) {
+     645           0 :     return;
+     646             :   }
+     647             : 
+     648      320919 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::callbackAltitudeAmsl", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     649             : 
+     650      213946 :   geometry_msgs::TransformStamped tf_msg;
+     651      106973 :   tf_msg.header.stamp    = msg->stamp;
+     652      106973 :   tf_msg.header.frame_id = ch_->frames.ns_fcu_untilted;
+     653      106973 :   tf_msg.child_frame_id  = ch_->frames.ns_amsl;
+     654             : 
+     655      106973 :   tf_msg.transform.translation.x = 0;
+     656      106973 :   tf_msg.transform.translation.y = 0;
+     657      106973 :   tf_msg.transform.translation.z = -msg->amsl;
+     658      106973 :   tf_msg.transform.rotation.x    = 0;
+     659      106973 :   tf_msg.transform.rotation.y    = 0;
+     660      106973 :   tf_msg.transform.rotation.z    = 0;
+     661      106973 :   tf_msg.transform.rotation.w    = 1;
+     662             : 
+     663      106973 :   if (Support::noNans(tf_msg)) {
+     664             :     try {
+     665      106932 :       broadcaster_->sendTransform(tf_msg);
+     666             :     }
+     667           0 :     catch (...) {
+     668           0 :       ROS_ERROR("exception caught ");
+     669             :     }
+     670             :   } else {
+     671          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(),
+     672             :                       tf_msg.child_frame_id.c_str());
+     673             :   }
+     674      106973 :   ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     675             :                 tf_msg.child_frame_id.c_str());
+     676             : }
+     677             : /*//}*/
+     678             : 
+     679             : /*//{ callbackHwApiOrientation() */
+     680      235808 : void TransformManager::callbackHwApiOrientation(const geometry_msgs::QuaternionStamped::ConstPtr msg) {
+     681             : 
+     682      235808 :   if (!is_initialized_) {
+     683           0 :     return;
+     684             :   }
+     685             : 
+     686      707424 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::publishFcuUntilted", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     687             : 
+     688      235808 :   if (publish_fcu_untilted_tf_) {
+     689      235808 :     publishFcuUntiltedTf(msg);
+     690             :   }
+     691             : }
+     692             : /*//}*/
+     693             : 
+     694             : /*//{ callbackGnss() */
+     695       82187 : void TransformManager::callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg) {
+     696             : 
+     697       82187 :   if (!is_initialized_) {
+     698       82081 :     return;
+     699             :   }
+     700             : 
+     701       82187 :   if (got_utm_offset_) {
+     702       82081 :     return;
+     703             :   }
+     704             : 
+     705         212 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::callbackGnss", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     706             : 
+     707             :   double out_x;
+     708             :   double out_y;
+     709             : 
+     710         106 :   mrs_lib::UTM(msg->latitude, msg->longitude, &out_x, &out_y);
+     711             : 
+     712         106 :   if (!std::isfinite(out_x)) {
+     713           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in UTM variable \"out_x\"!!!", getPrintName().c_str());
+     714           0 :     return;
+     715             :   }
+     716             : 
+     717         106 :   if (!std::isfinite(out_y)) {
+     718           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in UTM variable \"out_y\"!!!", getPrintName().c_str());
+     719           0 :     return;
+     720             :   }
+     721             : 
+     722         106 :   geometry_msgs::Point utm_origin;
+     723         106 :   utm_origin.x = out_x;
+     724         106 :   utm_origin.y = out_y;
+     725         106 :   utm_origin.z = msg->altitude;
+     726             : 
+     727         106 :   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);
+     728             : 
+     729         217 :   for (size_t i = 0; i < tf_sources_.size(); i++) {
+     730         111 :     tf_sources_[i]->setUtmOrigin(utm_origin);
+     731         111 :     tf_sources_[i]->setWorldOrigin(world_origin_);
+     732             :   }
+     733         106 :   got_utm_offset_ = true;
+     734             : }
+     735             : /*//}*/
+     736             : 
+     737             : /*//{ callbackRtkGps() */
+     738        2732 : void TransformManager::callbackRtkGps(const mrs_msgs::RtkGps::ConstPtr msg) {
+     739             : 
+     740        2732 :   if (!is_initialized_) {
+     741        2729 :     return;
+     742             :   }
+     743             : 
+     744        2732 :   if (got_utm_offset_) {
+     745        2727 :     return;
+     746             :   }
+     747             : 
+     748          10 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::callbackRtkGps", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     749             : 
+     750             :   double out_x;
+     751             :   double out_y;
+     752             : 
+     753           5 :   geometry_msgs::PoseStamped rtk_pos;
+     754             : 
+     755           5 :   if (!std::isfinite(msg->gps.latitude)) {
+     756           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in RTK variable \"msg->latitude\"!!!", getPrintName().c_str());
+     757           0 :     return;
+     758             :   }
+     759             : 
+     760           5 :   if (!std::isfinite(msg->gps.longitude)) {
+     761           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in RTK variable \"msg->longitude\"!!!", getPrintName().c_str());
+     762           0 :     return;
+     763             :   }
+     764             : 
+     765           5 :   if (!std::isfinite(msg->gps.altitude)) {
+     766           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in RTK variable \"msg->altitude\"!!!", getPrintName().c_str());
+     767           0 :     return;
+     768             :   }
+     769             : 
+     770           5 :   if (msg->fix_type.fix_type != mrs_msgs::RtkFixType::RTK_FIX) {
+     771           0 :     ROS_INFO_THROTTLE(1.0, "[%s] %s RTK FIX", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     772           0 :     return;
+     773             :   }
+     774             : 
+     775           5 :   rtk_pos.header = msg->header;
+     776           5 :   mrs_lib::UTM(msg->gps.latitude, msg->gps.longitude, &rtk_pos.pose.position.x, &rtk_pos.pose.position.y);
+     777           5 :   rtk_pos.pose.position.z  = msg->gps.altitude;
+     778           5 :   rtk_pos.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+     779             : 
+     780           5 :   rtk_pos.pose.position.x -= ch_->world_origin.x;
+     781           5 :   rtk_pos.pose.position.y -= ch_->world_origin.y;
+     782             : 
+     783             : 
+     784             :   // transform the RTK position from antenna to FCU
+     785           5 :   auto res = transformRtkToFcu(rtk_pos);
+     786           5 :   if (res) {
+     787           3 :     rtk_pos.pose = res.value();
+     788             :   } else {
+     789           2 :     ROS_ERROR_THROTTLE(1.0, "[%s]: transform to fcu failed", getPrintName().c_str());
+     790           2 :     return;
+     791             :   }
+     792             : 
+     793           3 :   mrs_lib::UTM(msg->gps.latitude, msg->gps.longitude, &out_x, &out_y);
+     794             : 
+     795           3 :   if (!std::isfinite(out_x)) {
+     796           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in UTM variable \"out_x\"!!!", getPrintName().c_str());
+     797           0 :     return;
+     798             :   }
+     799             : 
+     800           3 :   if (!std::isfinite(out_y)) {
+     801           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in UTM variable \"out_y\"!!!", getPrintName().c_str());
+     802           0 :     return;
+     803             :   }
+     804             : 
+     805           3 :   geometry_msgs::Point utm_origin;
+     806           3 :   utm_origin.x = out_x;
+     807           3 :   utm_origin.y = out_y;
+     808           3 :   utm_origin.z = msg->gps.altitude;
+     809             : 
+     810           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);
+     811             : 
+     812           9 :   for (size_t i = 0; i < tf_sources_.size(); i++) {
+     813           6 :     tf_sources_[i]->setUtmOrigin(utm_origin);
+     814           6 :     tf_sources_[i]->setWorldOrigin(world_origin_);
+     815             :   }
+     816           3 :   got_utm_offset_ = true;
+     817             : }
+     818             : /*//}*/
+     819             : 
+     820             : /*//{ publishFcuUntiltedTf() */
+     821      235808 : void TransformManager::publishFcuUntiltedTf(const geometry_msgs::QuaternionStampedConstPtr& msg) {
+     822             : 
+     823      471616 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::publishFcuUntilted", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     824             : 
+     825             :   double heading;
+     826             : 
+     827             :   try {
+     828      235808 :     heading = mrs_lib::AttitudeConverter(msg->quaternion).getHeading();
+     829             :   }
+     830           0 :   catch (...) {
+     831           0 :     ROS_ERROR("[%s]: Exception caught during getting heading", getPrintName().c_str());
+     832           0 :     return;
+     833             :   }
+     834      235808 :   scope_timer.checkpoint("heading");
+     835             : 
+     836      235808 :   const Eigen::Matrix3d odom_pixhawk_R = mrs_lib::AttitudeConverter(msg->quaternion);
+     837      235808 :   const Eigen::Matrix3d undo_heading_R = mrs_lib::AttitudeConverter(Eigen::AngleAxis(-heading, Eigen::Vector3d(0, 0, 1)));
+     838             : 
+     839      235808 :   const tf2::Quaternion q     = mrs_lib::AttitudeConverter(undo_heading_R * odom_pixhawk_R);
+     840      235808 :   const tf2::Quaternion q_inv = q.inverse();
+     841             : 
+     842      235808 :   scope_timer.checkpoint("q inverse");
+     843             : 
+     844      235808 :   geometry_msgs::TransformStamped tf;
+     845      235808 :   tf.header.stamp            = msg->header.stamp;  // TODO(petrlmat) ros::Time::now()?
+     846      235808 :   tf.header.frame_id         = ch_->frames.ns_fcu;
+     847      235808 :   tf.child_frame_id          = ch_->frames.ns_fcu_untilted;
+     848      235808 :   tf.transform.translation.x = 0.0;
+     849      235808 :   tf.transform.translation.y = 0.0;
+     850      235808 :   tf.transform.translation.z = 0.0;
+     851      235808 :   tf.transform.rotation      = mrs_lib::AttitudeConverter(q_inv);
+     852             : 
+     853      235808 :   scope_timer.checkpoint("tf fill");
+     854      235808 :   if (Support::noNans(tf)) {
+     855      235808 :     broadcaster_->sendTransform(tf);
+     856             :   } else {
+     857           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN encountered in fcu_untilted tf", getPrintName().c_str());
+     858             :   }
+     859      235808 :   scope_timer.checkpoint("tf pub");
+     860             : }
+     861             : /*//}*/
+     862             : 
+     863             : /* publishLocalTf() //{*/
+     864         109 : void TransformManager::publishLocalTf() {
+     865             : 
+     866         327 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishLocalTf", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     867             : 
+     868         109 :   geometry_msgs::TransformStamped tf_msg;
+     869         109 :   tf_msg.header.stamp = pose_first_.header.stamp;
+     870             : 
+     871         109 :   tf_msg.header.frame_id       = ns_fixed_origin_child_frame_id_;
+     872         109 :   tf_msg.child_frame_id        = ns_local_origin_child_frame_id_;
+     873         109 :   tf_msg.transform.translation = Support::pointToVector3(pose_first_.pose.position);
+     874         109 :   tf_msg.transform.rotation    = pose_first_.pose.orientation;
+     875             : 
+     876         109 :   if (Support::noNans(tf_msg)) {
+     877             : 
+     878             :     try {
+     879         109 :       static_broadcaster_.sendTransform(tf_msg);
+     880             :     }
+     881           0 :     catch (...) {
+     882           0 :       ROS_ERROR("[%s]: exception caught while publishing tf from %s to %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     883             :                 tf_msg.child_frame_id.c_str());
+     884             :     }
+     885             : 
+     886             :   } else {
+     887           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(),
+     888             :                       tf_msg.child_frame_id.c_str());
+     889             :   }
+     890         109 :   ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     891             :                 tf_msg.child_frame_id.c_str());
+     892         109 :   is_local_static_tf_published_ = true;
+     893         109 : }
+     894             : /*//}*/
+     895             : 
+     896             : /*//{ transformRtkToFcu() */
+     897           5 : std::optional<geometry_msgs::Pose> TransformManager::transformRtkToFcu(const geometry_msgs::PoseStamped& pose_in) const {
+     898             : 
+     899          10 :   geometry_msgs::PoseStamped pose_tmp = pose_in;
+     900             : 
+     901             :   // inject current orientation into rtk pose
+     902          10 :   auto res1 = ch_->transformer->getTransform(ch_->frames.ns_fcu_untilted, ch_->frames.ns_fcu, ros::Time::now());
+     903           5 :   if (res1) {
+     904           3 :     pose_tmp.pose.orientation = res1.value().transform.rotation;
+     905             :   } else {
+     906           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(),
+     907             :                        ch_->frames.ns_fcu.c_str());
+     908           2 :     return {};
+     909             :   }
+     910             : 
+     911             :   // invert tf
+     912           3 :   tf2::Transform             tf_utm_to_antenna = Support::tf2FromPose(pose_tmp.pose);
+     913           6 :   geometry_msgs::PoseStamped utm_in_antenna;
+     914           3 :   utm_in_antenna.pose            = Support::poseFromTf2(tf_utm_to_antenna.inverse());
+     915           3 :   utm_in_antenna.header.stamp    = pose_in.header.stamp;
+     916           3 :   utm_in_antenna.header.frame_id = ch_->frames.ns_rtk_antenna;
+     917             : 
+     918             :   // transform to fcu
+     919           6 :   geometry_msgs::PoseStamped utm_in_fcu;
+     920           3 :   utm_in_fcu.header.frame_id = ch_->frames.ns_fcu;
+     921           3 :   utm_in_fcu.header.stamp    = pose_in.header.stamp;
+     922           6 :   auto res2                  = ch_->transformer->transformSingle(utm_in_antenna, ch_->frames.ns_fcu);
+     923             : 
+     924           3 :   if (res2) {
+     925           3 :     utm_in_fcu = res2.value();
+     926             :   } else {
+     927           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(),
+     928             :                        ch_->frames.ns_fcu.c_str());
+     929           0 :     return {};
+     930             :   }
+     931             : 
+     932             :   // invert tf
+     933           3 :   tf2::Transform      tf_fcu_to_utm = Support::tf2FromPose(utm_in_fcu.pose);
+     934           3 :   geometry_msgs::Pose fcu_in_utm    = Support::poseFromTf2(tf_fcu_to_utm.inverse());
+     935             : 
+     936           3 :   return fcu_in_utm;
+     937             : }
+     938             : /*//}*/
+     939             : 
+     940             : /*//{ getName() */
+     941           0 : std::string TransformManager::getName() const {
+     942           0 :   return name_;
+     943             : }
+     944             : /*//}*/
+     945             : 
+     946             : /*//{ getPrintName() */
+     947        1756 : std::string TransformManager::getPrintName() const {
+     948        1756 :   return nodelet_name_;
+     949             : }
+     950             : /*//}*/
+     951             : 
+     952             : }  // namespace transform_manager
+     953             : 
+     954             : }  // namespace mrs_uav_managers
+     955             : 
+     956             : #include <pluginlib/class_list_macros.h>
+     957         109 : 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..40fab38fd5 --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.overview.html @@ -0,0 +1,260 @@ + + + + + + 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 + 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..a3013f0669b7f34fdc7955abbd3b2af2ab72a5fc GIT binary patch literal 3411 zcmV-Z4XpBsP)pV3_N8yNaP|>FdfI^K>rso!FTg` zu3xVdE-RD=vJ^A?q~YPaX8P_BhVpn+m!+ph_cD;>kpiWWNo`OxUWhbArogL)&H2sKjmB?Cdf8qL>2)V@QQ5V6TAz=bJV95r=sIwOziD;7y&WyU zjd0I6^6CK^8Z*#9qAQ~(cAZUNY;2cAlp8QI?hh;?CgxWMTIDtjM!eWKMLffD7quGmvRM; z0__xG(}2Ac6Qi@hW;ewdY4cc$Bz;n#nSM*uBdckb!e{#8kb1C27>)D76nkH3+vmz=AiaRVJ$ei-+Xb_Q20i!aaWtMb7BgFcRY8k;QhRJ!_|s^QJ*!#Jj;nM` z^tc1q8X2R)(b}UNlwyUium9q5@{GTG#)amc3csXcz~o3Qg+Pawo;8{|BP^ck$SC9jvnEVRjMzohz~!&>gQ_U z_B#GC0bOxG3783nyn6C9$W!(42)(3G6wpV}bV3V#A=4uQT3wvAO~w;iokyH&zK}=t zgjVMf125oFZ>6510i3MB2;FmBAhonNJN^9n1`8(}H9kQQh%rIdopz2Au_XD&lZ2lcINaj4itx|i&UNP zU6k}}UQC@B#*#FSd}xTfc0#fT0bQinI)ThxH-V9&Tza`cw(5P9%RpmG6tzp8{!jfW z61e~}{CP*xMZbF5b94^;pn8Np8TJaUt83s3X9qXm`PCeOQ%hzSrKTwkv1?CJAD4OQ z+NE5OR%8mdxHBZ_j6LlO)}N}#rnTZ;TGXJalvW|5AxA*tz)ZKrmfP>xXu`_+qqS?H zEnMc4p3Ti&Wji3`QWfrv!Z6+!8byxVg$drXnhoW#1$x)`s6*CcIgASpE*dOwvH{DW z1Yk8tAe1r#7t}RH*1_34v$oMy!8VNOq%`Dwsw+M?xu0s&(1?3GC4Od;MCkghsrVu6 z;KoqBm$UM*=r^gLGBax6u8!Dsp{^~T4@`<(Pr3AQS#k#_TUFwdGHRAS~J`@_Jk(^=;H2H;gOu2Z3jP&dygZ{75qpwzOgP31ZP5= zm%Rc)JfG6u;)2*UBvaW<98BZ(C9h`74l*oK;MptN{}m&S18$}2Oi>30##fYL0g~VH zmHQaz=LqMMdemcco1UV+?SNg^yz@X)g+?&ht)^5rw;M5TZ~NRwJkVmgn@QU0ka5qk zYyT?M#;!i6g;OLn?LoUX&KaQYxtLlw(TEl8Gum>Iv7t8>&BfPPgl7p?e!_Xw8s9`< zTjf1ViZwbreXBrbjCPk4dDmG^k+3_dnldVrNTVGF*^hC$wUE;fWD#K!Lw2dF!V7A3 z`CQ@bLy`_2qu%K1Q+|>4XyftM_dPtY-uLXirP1LuGS;l_2L()N=i2B z+I5`jn|f4f@Nm%=R*eVR@)Qr7hkFn|BYHRwG_9n!or}Hb;qa-H;^BPPUGq8&Uokc6 z;Xu}vj7_zWzR`W$@mgFaVfWu+Z)0IJcqaj z4J8|rURUXd%a#BQCe({*5;S2xJaFC6vifq zQi(-k1x}Q7#CCqx^|FF|Ap>7{G@l`z<QPI_Jv)_%o#Xm)|p_megc|EcKR<0M#H4 zQ=|oC63}VJyI&Z=3m*dVV$=Ivlab=@0*$TfywDOB3U#DB(756n!@@^(3Lu1qJqfn{ z2Qa8%AxeE}Vd2B>XN+NCSt-ca)E53e4~N{xIYkJ9`=~Cgf89q*>RNwR_tB=f$9){h z0T5v>b000>*L~DoFID4#f5?5{eE1UYf2IdI<%11WQAu<}k}hBOp*j%BD^S1i^$KRG zRmthuheFX>2=ozV*4UwNW>)F^n)*ktaMMA~m6ca&&B=RtJbNnK?$+f*clzFC?@tK>77k@8O~7pvd z&57+r*G&@Bp%hcpAjl*DYsGhbD_QESb!p%;^0>N>V|P+`G42tN*Sg8H$o{rn!4hs64N*> z1UOSi)^9ruUO|08L@fHUT95H$2tKMMUh7HseDeFtHk$R6k;d9Ogy6bD`YBvPFa!5= z?YPIA#1ST4%U4Qw9{DOaPmi34H_lzdpTU}+qk%aqd?}IwU5i~;cS9Dm4%Uz6Z9bsp z3ZN72*hHYQF*yJc2rmt26sy@K0j+ynVc%1}j3qwhb?*Wo?=0Tx;?i^H#s+D|#j!cQ zQ(c_5y=yC7a8!GOF#PQ>#{iubm{q|wvOZQ6cL`%I4lz!SK;oAA`X#-2&W6o%`|12wJyOdjPFI+yP_VNJ_tPtj7U-p~5NT pDaGSF-tXl3>Jn9HDaOq${{fuRRw=}!=*<8C002ovPDHLkV1j7MmF)lk 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..28f07058c7 --- /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:792118067.1 %
Date:2024-12-17 22:35:29Functions: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()21
mrs_uav_managers::uav_manager::UavManager::callbackTakeoff(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)23
mrs_uav_managers::uav_manager::UavManager::ungripSrv()23
mrs_uav_managers::uav_manager::UavManager::timerMidairActivation(ros::TimerEvent const&)80
mrs_uav_managers::uav_manager::UavManager::offboardSrv(bool)81
mrs_uav_managers::uav_manager::UavManager::midairActivationImpl[abi:cxx11]()82
mrs_uav_managers::uav_manager::UavManager::callbackMidairActivation(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)82
mrs_uav_managers::uav_manager::UavManager::toggleControlOutput(bool const&)85
mrs_uav_managers::uav_manager::UavManager::emergencyReferenceSrv(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)101
mrs_uav_managers::uav_manager::UavManager::setControlCallbacksSrv(bool const&)102
(anonymous namespace)::ProxyExec0::ProxyExec0()109
mrs_uav_managers::uav_manager::UavManager::initialize()109
mrs_uav_managers::uav_manager::UavManager::preinitialize()109
mrs_uav_managers::uav_manager::UavManager::onInit()109
mrs_uav_managers::uav_manager::UavManager::setOdometryCallbacksSrv(bool const&)129
mrs_uav_managers::uav_manager::UavManager::timerHwApiCapabilities(ros::TimerEvent const&)158
mrs_uav_managers::uav_manager::UavManager::switchControllerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)213
mrs_uav_managers::uav_manager::UavManager::switchTrackerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)214
mrs_uav_managers::uav_manager::UavManager::timerFlightTime(ros::TimerEvent const&)294
mrs_uav_managers::uav_manager::UavManager::timerLanding(ros::TimerEvent const&)774
mrs_uav_managers::uav_manager::UavManager::timerTakeoff(ros::TimerEvent const&)1038
mrs_uav_managers::uav_manager::UavManager::timerDiagnostics(ros::TimerEvent const&)2259
mrs_uav_managers::uav_manager::UavManager::timerMinHeight(ros::TimerEvent const&)22758
mrs_uav_managers::uav_manager::UavManager::timerMaxHeight(ros::TimerEvent const&)22827
mrs_uav_managers::uav_manager::UavManager::timerMaxthrottle(ros::TimerEvent const&)58170
mrs_uav_managers::uav_manager::UavManager::callbackHwApiGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)80568
mrs_uav_managers::uav_manager::UavManager::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)176140
+
+
+ + + +
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..bd318f7175 --- /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:792118067.1 %
Date:2024-12-17 22:35:29Functions:3838100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()109
mrs_uav_managers::uav_manager::UavManager::initialize()109
mrs_uav_managers::uav_manager::UavManager::takeoffSrv()21
mrs_uav_managers::uav_manager::UavManager::offboardSrv(bool)81
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&)774
mrs_uav_managers::uav_manager::UavManager::timerTakeoff(ros::TimerEvent const&)1038
mrs_uav_managers::uav_manager::UavManager::preinitialize()109
mrs_uav_managers::uav_manager::UavManager::timerMaxHeight(ros::TimerEvent const&)22827
mrs_uav_managers::uav_manager::UavManager::timerMinHeight(ros::TimerEvent const&)22758
mrs_uav_managers::uav_manager::UavManager::callbackTakeoff(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)23
mrs_uav_managers::uav_manager::UavManager::timerFlightTime(ros::TimerEvent const&)294
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>)176140
mrs_uav_managers::uav_manager::UavManager::switchTrackerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)214
mrs_uav_managers::uav_manager::UavManager::timerDiagnostics(ros::TimerEvent const&)2259
mrs_uav_managers::uav_manager::UavManager::timerMaxthrottle(ros::TimerEvent const&)58170
mrs_uav_managers::uav_manager::UavManager::callbackHwApiGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)80568
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&)213
mrs_uav_managers::uav_manager::UavManager::toggleControlOutput(bool const&)85
mrs_uav_managers::uav_manager::UavManager::midairActivationImpl[abi:cxx11]()82
mrs_uav_managers::uav_manager::UavManager::emergencyReferenceSrv(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)101
mrs_uav_managers::uav_manager::UavManager::timerMidairActivation(ros::TimerEvent const&)80
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&)102
mrs_uav_managers::uav_manager::UavManager::timerHwApiCapabilities(ros::TimerEvent const&)158
mrs_uav_managers::uav_manager::UavManager::setOdometryCallbacksSrv(bool const&)129
mrs_uav_managers::uav_manager::UavManager::callbackMidairActivation(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)82
mrs_uav_managers::uav_manager::UavManager::onInit()109
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..ac58629995 --- /dev/null +++ b/mrs_uav_managers/src/uav_manager.cpp.gcov.html @@ -0,0 +1,2786 @@ + + + + + + + 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:792118067.1 %
Date:2024-12-17 22:35:29Functions: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         109 : void UavManager::onInit() {
+     317         109 :   preinitialize();
+     318         109 : }
+     319             : 
+     320             : //}
+     321             : 
+     322             : /* preinitialize() //{ */
+     323             : 
+     324         109 : void UavManager::preinitialize(void) {
+     325             : 
+     326         109 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     327             : 
+     328         109 :   ros::Time::waitForValid();
+     329             : 
+     330         109 :   mrs_lib::SubscribeHandlerOptions shopts;
+     331         109 :   shopts.nh                 = nh_;
+     332         109 :   shopts.node_name          = "UavManager";
+     333         109 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     334         109 :   shopts.threadsafe         = true;
+     335         109 :   shopts.autostart          = true;
+     336         109 :   shopts.queue_size         = 10;
+     337         109 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     338             : 
+     339         109 :   sh_hw_api_capabilities_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities>(shopts, "hw_api_capabilities_in");
+     340             : 
+     341         109 :   timer_hw_api_capabilities_ = nh_.createTimer(ros::Rate(1.0), &UavManager::timerHwApiCapabilities, this);
+     342         109 : }
+     343             : 
+     344             : //}
+     345             : 
+     346             : /* initialize() //{ */
+     347             : 
+     348         109 : void UavManager::initialize() {
+     349             : 
+     350         109 :   ROS_INFO("[UavManager]: initializing");
+     351             : 
+     352         218 :   mrs_lib::ParamLoader param_loader(nh_, "UavManager");
+     353             : 
+     354         218 :   std::string custom_config_path;
+     355         218 :   std::string platform_config_path;
+     356         218 :   std::string world_config_path;
+     357             : 
+     358         109 :   param_loader.loadParam("custom_config", custom_config_path);
+     359         109 :   param_loader.loadParam("platform_config", platform_config_path);
+     360         109 :   param_loader.loadParam("world_config", world_config_path);
+     361             : 
+     362         109 :   if (custom_config_path != "") {
+     363         109 :     param_loader.addYamlFile(custom_config_path);
+     364             :   }
+     365             : 
+     366         109 :   if (platform_config_path != "") {
+     367         109 :     param_loader.addYamlFile(platform_config_path);
+     368             :   }
+     369             : 
+     370         109 :   if (world_config_path != "") {
+     371         109 :     param_loader.addYamlFile(world_config_path);
+     372             :   }
+     373             : 
+     374         109 :   param_loader.addYamlFileFromParam("private_config");
+     375         109 :   param_loader.addYamlFileFromParam("public_config");
+     376             : 
+     377         218 :   const std::string yaml_prefix = "mrs_uav_managers/uav_manager/";
+     378             : 
+     379             :   // params passed from the launch file are not prefixed
+     380         109 :   param_loader.loadParam("uav_name", _uav_name_);
+     381         109 :   param_loader.loadParam("enable_profiler", _profiler_enabled_);
+     382         109 :   param_loader.loadParam("uav_mass", _uav_mass_);
+     383         109 :   param_loader.loadParam("g", _g_);
+     384             : 
+     385             :   // motor params are also not prefixed, since they are common to more nodes
+     386         109 :   param_loader.loadParam("motor_params/n_motors", _throttle_model_.n_motors);
+     387         109 :   param_loader.loadParam("motor_params/a", _throttle_model_.A);
+     388         109 :   param_loader.loadParam("motor_params/b", _throttle_model_.B);
+     389             : 
+     390         109 :   param_loader.loadParam(yaml_prefix + "null_tracker", _null_tracker_name_);
+     391             : 
+     392         109 :   param_loader.loadParam(yaml_prefix + "takeoff/rate", _takeoff_timer_rate_);
+     393         109 :   param_loader.loadParam(yaml_prefix + "takeoff/after_takeoff/tracker", _after_takeoff_tracker_name_);
+     394         109 :   param_loader.loadParam(yaml_prefix + "takeoff/after_takeoff/controller", _after_takeoff_controller_name_);
+     395         109 :   param_loader.loadParam(yaml_prefix + "takeoff/during_takeoff/controller", _takeoff_controller_name_);
+     396         109 :   param_loader.loadParam(yaml_prefix + "takeoff/during_takeoff/tracker", _takeoff_tracker_name_);
+     397         109 :   param_loader.loadParam(yaml_prefix + "takeoff/takeoff_height", _takeoff_height_);
+     398             : 
+     399         109 :   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         109 :   param_loader.loadParam(yaml_prefix + "landing/rate", _landing_timer_rate_);
+     405         109 :   param_loader.loadParam(yaml_prefix + "landing/landing_tracker", _landing_tracker_name_);
+     406         109 :   param_loader.loadParam(yaml_prefix + "landing/landing_controller", _landing_controller_name_);
+     407         109 :   param_loader.loadParam(yaml_prefix + "landing/landing_cutoff_mass_factor", _landing_cutoff_mass_factor_);
+     408         109 :   param_loader.loadParam(yaml_prefix + "landing/landing_cutoff_timeout", _landing_cutoff_mass_timeout_);
+     409         109 :   param_loader.loadParam(yaml_prefix + "landing/disarm", _landing_disarm_);
+     410         109 :   param_loader.loadParam(yaml_prefix + "landing/descend_height", _landing_descend_height_);
+     411         109 :   param_loader.loadParam(yaml_prefix + "landing/tracking_tolerance/translation", _landing_tracking_tolerance_translation_);
+     412         109 :   param_loader.loadParam(yaml_prefix + "landing/tracking_tolerance/heading", _landing_tracking_tolerance_heading_);
+     413             : 
+     414         109 :   param_loader.loadParam(yaml_prefix + "midair_activation/rate", _midair_activation_timer_rate_);
+     415         109 :   param_loader.loadParam(yaml_prefix + "midair_activation/during_activation/controller", _midair_activation_during_controller_);
+     416         109 :   param_loader.loadParam(yaml_prefix + "midair_activation/during_activation/tracker", _midair_activation_during_tracker_);
+     417         109 :   param_loader.loadParam(yaml_prefix + "midair_activation/after_activation/controller", _midair_activation_after_controller_);
+     418         109 :   param_loader.loadParam(yaml_prefix + "midair_activation/after_activation/tracker", _midair_activation_after_tracker_);
+     419             : 
+     420         109 :   param_loader.loadParam(yaml_prefix + "max_height_checking/enabled", _max_height_enabled_);
+     421         109 :   param_loader.loadParam(yaml_prefix + "max_height_checking/rate", _max_height_checking_rate_);
+     422         109 :   param_loader.loadParam(yaml_prefix + "max_height_checking/safety_height_offset", _max_height_offset_);
+     423             : 
+     424             :   {
+     425             :     bool tmp;
+     426             : 
+     427         109 :     param_loader.loadParam(yaml_prefix + "min_height_checking/enabled", tmp);
+     428             : 
+     429         109 :     min_height_check_ = tmp;
+     430             :   }
+     431             : 
+     432         109 :   param_loader.loadParam(yaml_prefix + "min_height_checking/rate", _min_height_checking_rate_);
+     433         109 :   param_loader.loadParam(yaml_prefix + "min_height_checking/safety_height_offset", _min_height_offset_);
+     434         109 :   param_loader.loadParam(yaml_prefix + "min_height_checking/min_height", _min_height_);
+     435             : 
+     436         109 :   param_loader.loadParam(yaml_prefix + "require_gain_manager", _gain_manager_required_);
+     437         109 :   param_loader.loadParam(yaml_prefix + "require_constraint_manager", _constraint_manager_required_);
+     438             : 
+     439         109 :   param_loader.loadParam(yaml_prefix + "flight_timer/enabled", _flighttime_timer_enabled_);
+     440         109 :   param_loader.loadParam(yaml_prefix + "flight_timer/rate", _flighttime_timer_rate_);
+     441         109 :   param_loader.loadParam(yaml_prefix + "flight_timer/max_time", _flighttime_max_time_);
+     442             : 
+     443         109 :   param_loader.loadParam(yaml_prefix + "max_throttle/enabled", _maxthrottle_timer_enabled_);
+     444         109 :   param_loader.loadParam(yaml_prefix + "max_throttle/rate", _maxthrottle_timer_rate_);
+     445         109 :   param_loader.loadParam(yaml_prefix + "max_throttle/max_throttle", _maxthrottle_max_throttle_);
+     446         109 :   param_loader.loadParam(yaml_prefix + "max_throttle/eland_timeout", _maxthrottle_eland_timeout_);
+     447         109 :   param_loader.loadParam(yaml_prefix + "max_throttle/ungrip_timeout", _maxthrottle_ungrip_timeout_);
+     448             : 
+     449         109 :   param_loader.loadParam(yaml_prefix + "diagnostics/rate", _diagnostics_timer_rate_);
+     450             : 
+     451             :   // | ------------------- scope timer logger ------------------- |
+     452             : 
+     453         109 :   param_loader.loadParam(yaml_prefix + "scope_timer/enabled", scope_timer_enabled_);
+     454         327 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+     455         109 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+     456             : 
+     457         109 :   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         109 :   transformer_ = std::make_shared<mrs_lib::Transformer>(nh_, "ControlManager");
+     465         109 :   transformer_->setDefaultPrefix(_uav_name_);
+     466         109 :   transformer_->retryLookupNewest(true);
+     467             : 
+     468             :   // | ----------------------- subscribers ---------------------- |
+     469             : 
+     470         218 :   mrs_lib::SubscribeHandlerOptions shopts;
+     471         109 :   shopts.nh                 = nh_;
+     472         109 :   shopts.node_name          = "UavManager";
+     473         109 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     474         109 :   shopts.threadsafe         = true;
+     475         109 :   shopts.autostart          = true;
+     476         109 :   shopts.queue_size         = 10;
+     477         109 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     478             : 
+     479         109 :   sh_controller_diagnostics_ = mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics>(shopts, "controller_diagnostics_in");
+     480         109 :   sh_odometry_               = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, "odometry_in", &UavManager::callbackOdometry, this);
+     481         109 :   sh_estimation_diagnostics_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, "odometry_diagnostics_in");
+     482         109 :   sh_control_manager_diag_   = mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>(shopts, "control_manager_diagnostics_in");
+     483         109 :   sh_mass_estimate_          = mrs_lib::SubscribeHandler<std_msgs::Float64>(shopts, "mass_estimate_in");
+     484         109 :   sh_throttle_               = mrs_lib::SubscribeHandler<std_msgs::Float64>(shopts, "throttle_in");
+     485         109 :   sh_height_                 = mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>(shopts, "height_in");
+     486         109 :   sh_hw_api_status_          = mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus>(shopts, "hw_api_status_in");
+     487         109 :   sh_gains_diag_             = mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics>(shopts, "gain_manager_diagnostics_in");
+     488         109 :   sh_constraints_diag_       = mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics>(shopts, "constraint_manager_diagnostics_in");
+     489         109 :   sh_hw_api_gnss_            = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, "hw_api_gnss_in", &UavManager::callbackHwApiGNSS, this);
+     490         109 :   sh_max_height_             = mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>(shopts, "max_height_in");
+     491         109 :   sh_tracker_cmd_            = mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand>(shopts, "tracker_cmd_in");
+     492             : 
+     493             :   // | ----------------------- publishers ----------------------- |
+     494             : 
+     495         109 :   ph_diag_ = mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics>(nh_, "diagnostics_out", 1);
+     496             : 
+     497             :   // | --------------------- service servers -------------------- |
+     498             : 
+     499         109 :   service_server_takeoff_           = nh_.advertiseService("takeoff_in", &UavManager::callbackTakeoff, this);
+     500         109 :   service_server_land_              = nh_.advertiseService("land_in", &UavManager::callbackLand, this);
+     501         109 :   service_server_land_home_         = nh_.advertiseService("land_home_in", &UavManager::callbackLandHome, this);
+     502         109 :   service_server_land_there_        = nh_.advertiseService("land_there_in", &UavManager::callbackLandThere, this);
+     503         109 :   service_server_midair_activation_ = nh_.advertiseService("midair_activation_in", &UavManager::callbackMidairActivation, this);
+     504         109 :   service_server_min_height_check_  = nh_.advertiseService("enable_min_height_check_in", &UavManager::callbackMinHeightCheck, this);
+     505             : 
+     506             :   // | --------------------- service clients -------------------- |
+     507             : 
+     508         109 :   sch_takeoff_               = mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>(nh_, "takeoff_out");
+     509         109 :   sch_land_                  = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "land_out");
+     510         109 :   sch_eland_                 = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "eland_out");
+     511         109 :   sch_ehover_                = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "ehover_out");
+     512         109 :   sch_switch_tracker_        = mrs_lib::ServiceClientHandler<mrs_msgs::String>(nh_, "switch_tracker_out");
+     513         109 :   sch_switch_controller_     = mrs_lib::ServiceClientHandler<mrs_msgs::String>(nh_, "switch_controller_out");
+     514         109 :   sch_emergency_reference_   = mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>(nh_, "emergency_reference_out");
+     515         109 :   sch_control_callbacks_     = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "enable_callbacks_out");
+     516         109 :   sch_arm_                   = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "arm_out");
+     517         109 :   sch_odometry_callbacks_    = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "set_odometry_callbacks_out");
+     518         109 :   sch_ungrip_                = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "ungrip_out");
+     519         109 :   sch_toggle_control_output_ = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "toggle_control_output_out");
+     520         109 :   sch_offboard_              = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "offboard_out");
+     521             : 
+     522             :   // | ---------------------- state machine --------------------- |
+     523             : 
+     524         109 :   current_state_landing_ = IDLE_STATE;
+     525             : 
+     526             :   // | ------------------------ profiler ------------------------ |
+     527             : 
+     528         109 :   profiler_ = mrs_lib::Profiler(nh_, "UavManager", _profiler_enabled_);
+     529             : 
+     530             :   // | ------------------------- timers ------------------------- |
+     531             : 
+     532         109 :   timer_landing_           = nh_.createTimer(ros::Rate(_landing_timer_rate_), &UavManager::timerLanding, this, false, false);
+     533         109 :   timer_takeoff_           = nh_.createTimer(ros::Rate(_takeoff_timer_rate_), &UavManager::timerTakeoff, this, false, false);
+     534         109 :   timer_flighttime_        = nh_.createTimer(ros::Rate(_flighttime_timer_rate_), &UavManager::timerFlightTime, this, false, false);
+     535         109 :   timer_diagnostics_       = nh_.createTimer(ros::Rate(_diagnostics_timer_rate_), &UavManager::timerDiagnostics, this);
+     536         109 :   timer_midair_activation_ = nh_.createTimer(ros::Rate(_midair_activation_timer_rate_), &UavManager::timerMidairActivation, this, false, false);
+     537         218 :   timer_max_height_        = nh_.createTimer(ros::Rate(_max_height_checking_rate_), &UavManager::timerMaxHeight, this, false,
+     538         218 :                                       _max_height_enabled_ && hw_api_capabilities_.produces_distance_sensor);
+     539         218 :   timer_min_height_        = nh_.createTimer(ros::Rate(_min_height_checking_rate_), &UavManager::timerMinHeight, this, false,
+     540         218 :                                       min_height_check_ && hw_api_capabilities_.produces_distance_sensor);
+     541             : 
+     542         106 :   bool should_check_throttle = hw_api_capabilities_.accepts_actuator_cmd || hw_api_capabilities_.accepts_control_group_cmd ||
+     543         215 :                                hw_api_capabilities_.accepts_attitude_rate_cmd || hw_api_capabilities_.accepts_attitude_cmd;
+     544             : 
+     545             :   timer_maxthrottle_ =
+     546         109 :       nh_.createTimer(ros::Rate(_maxthrottle_timer_rate_), &UavManager::timerMaxthrottle, this, false, _maxthrottle_timer_enabled_ && should_check_throttle);
+     547             : 
+     548             :   // | ----------------------- finish init ---------------------- |
+     549             : 
+     550         109 :   is_initialized_ = true;
+     551             : 
+     552         109 :   ROS_INFO("[UavManager]: initialized");
+     553             : 
+     554         109 :   ROS_DEBUG("[UavManager]: debug output is enabled");
+     555         109 : }
+     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         158 : void UavManager::timerHwApiCapabilities(const ros::TimerEvent& event) {
+     603             : 
+     604         316 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerHwApiCapabilities", 1.0, 1.0, event);
+     605         316 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerHwApiCapabilities", scope_timer_logger_, scope_timer_enabled_);
+     606             : 
+     607         158 :   if (!sh_hw_api_capabilities_.hasMsg()) {
+     608          49 :     ROS_INFO_THROTTLE(1.0, "[UavManager]: waiting for HW API capabilities");
+     609          49 :     return;
+     610             :   }
+     611             : 
+     612         109 :   hw_api_capabilities_ = *sh_hw_api_capabilities_.getMsg();
+     613             : 
+     614         109 :   ROS_INFO("[UavManager]: got HW API capabilities, initializing");
+     615             : 
+     616         109 :   initialize();
+     617             : 
+     618         109 :   timer_hw_api_capabilities_.stop();
+     619             : }
+     620             : 
+     621             : //}
+     622             : 
+     623             : /* //{ timerLanding() */
+     624             : 
+     625         774 : void UavManager::timerLanding(const ros::TimerEvent& event) {
+     626             : 
+     627         774 :   if (!is_initialized_)
+     628           0 :     return;
+     629             : 
+     630        1548 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerLanding", _landing_timer_rate_, 0.1, event);
+     631        1548 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerLanding", scope_timer_logger_, scope_timer_enabled_);
+     632             : 
+     633         774 :   auto land_there_reference = mrs_lib::get_mutexed(mutex_land_there_reference_, land_there_reference_);
+     634             : 
+     635             :   // copy member variables
+     636         774 :   auto control_manager_diagnostics = sh_control_manager_diag_.getMsg();
+     637         774 :   auto odometry                    = sh_odometry_.getMsg();
+     638         774 :   auto tracker_cmd                 = sh_tracker_cmd_.getMsg();
+     639             : 
+     640         774 :   std::optional<double> desired_throttle;
+     641             : 
+     642         774 :   if (sh_throttle_.hasMsg() && (ros::Time::now() - sh_throttle_.lastMsgTime()).toSec() < 1.0) {
+     643         774 :     desired_throttle = sh_throttle_.getMsg()->data;
+     644             :   }
+     645             : 
+     646         774 :   auto res = transformer_->transformSingle(land_there_reference, odometry->header.frame_id);
+     647             : 
+     648         774 :   mrs_msgs::ReferenceStamped land_there_current_frame;
+     649             : 
+     650         774 :   if (res) {
+     651         774 :     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         774 :   if (current_state_landing_ == IDLE_STATE) {
+     659             : 
+     660           0 :     return;
+     661             : 
+     662         774 :   } else if (current_state_landing_ == GOTO_STATE) {
+     663             : 
+     664         344 :     auto [pos_x, pos_y, pos_z] = mrs_lib::getPosition(*tracker_cmd);
+     665         344 :     auto [ref_x, ref_y, ref_z] = mrs_lib::getPosition(land_there_current_frame);
+     666             : 
+     667         344 :     double pos_heading = tracker_cmd->heading;
+     668             : 
+     669         344 :     double ref_heading = 0;
+     670             :     try {
+     671         344 :       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         351 :     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         337 :     } else if (!control_manager_diagnostics->tracker_status.have_goal && control_manager_diagnostics->flying_normally) {
+     689             : 
+     690           0 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: the tracker does not have a goal while flying home, setting the reference again");
+     691             : 
+     692           0 :       mrs_msgs::ReferenceStamped reference_out;
+     693             : 
+     694             :       {
+     695           0 :         std::scoped_lock lock(mutex_land_there_reference_);
+     696             : 
+     697             :         // get the current altitude in land_there_reference_.header.frame_id;
+     698           0 :         geometry_msgs::PoseStamped current_pose;
+     699           0 :         current_pose.header.stamp     = ros::Time::now();
+     700           0 :         current_pose.header.frame_id  = _uav_name_ + "/fcu";
+     701           0 :         current_pose.pose.position.x  = 0;
+     702           0 :         current_pose.pose.position.y  = 0;
+     703           0 :         current_pose.pose.position.z  = 0;
+     704           0 :         current_pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+     705             : 
+     706           0 :         auto response = transformer_->transformSingle(current_pose, land_there_reference_.header.frame_id);
+     707             : 
+     708           0 :         if (response) {
+     709             : 
+     710           0 :           land_there_reference_.reference.position.z = response.value().pose.position.z;
+     711           0 :           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           0 :         reference_out.header.frame_id = land_there_reference_.header.frame_id;
+     721           0 :         reference_out.header.stamp    = ros::Time::now();
+     722           0 :         reference_out.reference       = land_there_reference_.reference;
+     723             :       }
+     724             : 
+     725           0 :       emergencyReferenceSrv(reference_out);
+     726             :     }
+     727             : 
+     728         430 :   } else if (current_state_landing_ == LANDING_STATE) {
+     729             : 
+     730             :     // we should not attempt to finish the landing if some other tracked was activated
+     731         430 :     if (_landing_tracker_name_ == sh_control_manager_diag_.getMsg()->active_tracker) {
+     732             : 
+     733         430 :       if (desired_throttle) {
+     734             : 
+     735             :         // recalculate the mass based on the throttle
+     736         430 :         throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(_throttle_model_, desired_throttle.value()) / _g_;
+     737         430 :         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         430 :         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         320 :           throttle_under_threshold_ = false;
+     753             :         }
+     754             : 
+     755         430 :         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        1038 : void UavManager::timerTakeoff(const ros::TimerEvent& event) {
+     832             : 
+     833        1038 :   if (!is_initialized_)
+     834           0 :     return;
+     835             : 
+     836        2076 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerTakeoff", _takeoff_timer_rate_, 0.1, event);
+     837        2076 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerTakeoff", scope_timer_logger_, scope_timer_enabled_);
+     838             : 
+     839        1038 :   auto control_manager_diagnostics = sh_control_manager_diag_.getMsg();
+     840             : 
+     841        1038 :   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          21 :       waiting_for_takeoff_ = false;
+     846             :     } else {
+     847             : 
+     848           0 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: waiting for takeoff confirmation from the ControlManager");
+     849           0 :       return;
+     850             :     }
+     851             :   }
+     852             : 
+     853        1038 :   if (takingoff_) {
+     854             : 
+     855        1038 :     if (control_manager_diagnostics->active_tracker != _takeoff_tracker_name_ || !control_manager_diagnostics->tracker_status.have_goal) {
+     856             : 
+     857          21 :       auto [odom_x, odom_y, odom_z] = mrs_lib::getPosition(sh_odometry_.getMsg());
+     858             : 
+     859             :       double odom_heading;
+     860             :       try {
+     861          21 :         odom_heading = mrs_lib::getHeading(sh_odometry_.getMsg());
+     862             :       }
+     863           0 :       catch (mrs_lib::AttitudeConverter::GetHeadingException& e) {
+     864           0 :         ROS_ERROR_THROTTLE(1.0, "[UavManager]: exception caught: '%s'", e.what());
+     865           0 :         return;
+     866             :       }
+     867             :       // this is needed for land_home to work with vins_kickoff estimator
+     868             :       // if there are any problems with this, it shoud be sufficient to only overwrite the frame_id, without the position and heading here
+     869             :       {
+     870          21 :         std::scoped_lock lock(mutex_land_there_reference_);
+     871          21 :         land_there_reference_.header               = sh_odometry_.getMsg()->header;
+     872          21 :         land_there_reference_.reference.position.x = odom_x;
+     873          21 :         land_there_reference_.reference.position.y = odom_y;
+     874          21 :         land_there_reference_.reference.position.z = odom_z;
+     875          21 :         land_there_reference_.reference.heading    = odom_heading;
+     876             :       }
+     877             : 
+     878          21 :       ROS_INFO("[UavManager]: take off finished, switching to %s", _after_takeoff_tracker_name_.c_str());
+     879             : 
+     880          21 :       switchTrackerSrv(_after_takeoff_tracker_name_);
+     881             : 
+     882          21 :       switchControllerSrv(_after_takeoff_controller_name_);
+     883             : 
+     884          21 :       setOdometryCallbacksSrv(true);
+     885             : 
+     886          21 :       timer_takeoff_.stop();
+     887             :     }
+     888             :   }
+     889             : }
+     890             : 
+     891             : //}
+     892             : 
+     893             : /* //{ timerMaxHeight() */
+     894             : 
+     895       22827 : void UavManager::timerMaxHeight(const ros::TimerEvent& event) {
+     896             : 
+     897       22827 :   if (!is_initialized_)
+     898       15453 :     return;
+     899             : 
+     900       45654 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerMaxHeight", _max_height_checking_rate_, 0.1, event);
+     901       45654 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerMaxHeight", scope_timer_logger_, scope_timer_enabled_);
+     902             : 
+     903       22827 :   if (!sh_max_height_.hasMsg() || !sh_height_.hasMsg() || !sh_odometry_.hasMsg()) {
+     904       10712 :     ROS_WARN_THROTTLE(10.0, "[UavManager]: maxHeightTimer() not spinning, missing data");
+     905       10712 :     return;
+     906             :   }
+     907             : 
+     908       12115 :   auto control_manager_diag = sh_control_manager_diag_.getMsg();
+     909             : 
+     910       12115 :   if (!fixing_max_height_ && !control_manager_diag->flying_normally) {
+     911        4741 :     return;
+     912             :   }
+     913             : 
+     914        7374 :   auto   odometry = sh_odometry_.getMsg();
+     915        7374 :   double height   = sh_height_.getMsg()->value;
+     916             : 
+     917             :   // transform max z to the height frame
+     918        7374 :   geometry_msgs::PointStamped point;
+     919        7374 :   point.header  = sh_max_height_.getMsg()->header;
+     920        7374 :   point.point.z = sh_max_height_.getMsg()->value;
+     921             : 
+     922        7374 :   auto res = transformer_->transformSingle(point, sh_height_.getMsg()->header.frame_id);
+     923             : 
+     924             :   double max_z_in_height;
+     925             : 
+     926        7374 :   if (res) {
+     927        7374 :     max_z_in_height = res->point.z;
+     928             :   } else {
+     929           0 :     ROS_WARN_THROTTLE(1.0, "[UavManager]: timerMaxHeight() not working, cannot transform max z to the height frame");
+     930           0 :     return;
+     931             :   }
+     932             : 
+     933        7374 :   auto [odometry_x, odometry_y, odometry_z] = mrs_lib::getPosition(odometry);
+     934             : 
+     935        7374 :   double odometry_heading = 0;
+     936             :   try {
+     937        7374 :     odometry_heading = mrs_lib::getHeading(odometry);
+     938             :   }
+     939           0 :   catch (mrs_lib::AttitudeConverter::GetHeadingException& e) {
+     940           0 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: exception caught: '%s'", e.what());
+     941           0 :     return;
+     942             :   }
+     943             : 
+     944        7374 :   if (height > max_z_in_height) {
+     945             : 
+     946          54 :     ROS_WARN_THROTTLE(1.0, "[UavManager]: max height exceeded: %.2f >  %.2f, triggering safety goto", height, max_z_in_height);
+     947             : 
+     948         108 :     mrs_msgs::ReferenceStamped reference_out;
+     949          54 :     reference_out.header.frame_id = odometry->header.frame_id;
+     950          54 :     reference_out.header.stamp    = ros::Time::now();
+     951             : 
+     952          54 :     reference_out.reference.position.x = odometry_x;
+     953          54 :     reference_out.reference.position.y = odometry_y;
+     954          54 :     reference_out.reference.position.z = odometry_z + ((max_z_in_height - _max_height_offset_) - height);
+     955             : 
+     956          54 :     reference_out.reference.heading = odometry_heading;
+     957             : 
+     958          54 :     setControlCallbacksSrv(false);
+     959             : 
+     960          54 :     bool success = emergencyReferenceSrv(reference_out);
+     961             : 
+     962          54 :     if (success) {
+     963             : 
+     964          54 :       ROS_INFO("[UavManager]: descending");
+     965             : 
+     966          54 :       fixing_max_height_ = true;
+     967             : 
+     968             :     } else {
+     969             : 
+     970           0 :       ROS_ERROR_THROTTLE(1.0, "[UavManager]: could not descend");
+     971             : 
+     972           0 :       setControlCallbacksSrv(true);
+     973             :     }
+     974             :   }
+     975             : 
+     976        7374 :   if (fixing_max_height_ && height < max_z_in_height) {
+     977             : 
+     978           1 :     setControlCallbacksSrv(true);
+     979             : 
+     980           1 :     ROS_WARN_THROTTLE(1.0, "[UavManager]: safe height reached");
+     981             : 
+     982           1 :     fixing_max_height_ = false;
+     983             :   }
+     984             : }
+     985             : 
+     986             : //}
+     987             : 
+     988             : /* //{ timerMinHeight() */
+     989             : 
+     990       22758 : void UavManager::timerMinHeight(const ros::TimerEvent& event) {
+     991             : 
+     992       22758 :   if (!is_initialized_)
+     993       15467 :     return;
+     994             : 
+     995       22758 :   ROS_INFO_ONCE("[UavManager]: min height timer spinning");
+     996             : 
+     997       45516 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerMinHeight", _min_height_checking_rate_, 0.1, event);
+     998       45516 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerMinHeight", scope_timer_logger_, scope_timer_enabled_);
+     999             : 
+    1000       22758 :   if (!sh_odometry_.hasMsg() || !sh_height_.hasMsg() || !sh_control_manager_diag_.hasMsg()) {
+    1001       10693 :     ROS_WARN_THROTTLE(10.0, "[UavManager]: minHeightTimer() not spinning, missing data");
+    1002       10693 :     return;
+    1003             :   }
+    1004             : 
+    1005       12065 :   auto control_manager_diag = sh_control_manager_diag_.getMsg();
+    1006             : 
+    1007       12065 :   if (!fixing_min_height_ && !control_manager_diag->flying_normally) {
+    1008        4774 :     return;
+    1009             :   }
+    1010             : 
+    1011        7291 :   auto   odometry = sh_odometry_.getMsg();
+    1012        7291 :   double height   = sh_height_.getMsg()->value;
+    1013             : 
+    1014        7291 :   auto [odometry_x, odometry_y, odometry_z] = mrs_lib::getPosition(odometry);
+    1015             : 
+    1016        7291 :   double odometry_heading = 0;
+    1017             : 
+    1018             :   try {
+    1019        7291 :     odometry_heading = mrs_lib::getHeading(odometry);
+    1020             :   }
+    1021           0 :   catch (mrs_lib::AttitudeConverter::GetHeadingException& e) {
+    1022           0 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: exception caught: '%s'", e.what());
+    1023           0 :     return;
+    1024             :   }
+    1025             : 
+    1026        7291 :   if (height < _min_height_) {
+    1027             : 
+    1028          40 :     ROS_WARN_THROTTLE(1.0, "[UavManager]: min height breached: %.2f < %.2f, triggering safety goto", height, _min_height_);
+    1029             : 
+    1030          80 :     mrs_msgs::ReferenceStamped reference_out;
+    1031          40 :     reference_out.header.frame_id = odometry->header.frame_id;
+    1032          40 :     reference_out.header.stamp    = ros::Time::now();
+    1033             : 
+    1034          40 :     reference_out.reference.position.x = odometry_x;
+    1035          40 :     reference_out.reference.position.y = odometry_y;
+    1036          40 :     reference_out.reference.position.z = odometry_z + ((_min_height_ + _min_height_offset_) - height);
+    1037             : 
+    1038          40 :     reference_out.reference.heading = odometry_heading;
+    1039             : 
+    1040          40 :     setControlCallbacksSrv(false);
+    1041             : 
+    1042          40 :     bool success = emergencyReferenceSrv(reference_out);
+    1043             : 
+    1044          40 :     if (success) {
+    1045             : 
+    1046          40 :       ROS_INFO("[UavManager]: ascending");
+    1047             : 
+    1048          40 :       fixing_min_height_ = true;
+    1049             : 
+    1050             :     } else {
+    1051             : 
+    1052           0 :       ROS_ERROR_THROTTLE(1.0, "[UavManager]: could not ascend");
+    1053             : 
+    1054           0 :       setControlCallbacksSrv(true);
+    1055             :     }
+    1056             :   }
+    1057             : 
+    1058        7291 :   if (fixing_min_height_ && height > _min_height_) {
+    1059             : 
+    1060           2 :     setControlCallbacksSrv(true);
+    1061             : 
+    1062           2 :     ROS_WARN_THROTTLE(1.0, "[UavManager]: safe height reached");
+    1063             : 
+    1064           2 :     fixing_min_height_ = false;
+    1065             :   }
+    1066             : }
+    1067             : 
+    1068             : //}
+    1069             : 
+    1070             : /* //{ timerFlightTime() */
+    1071             : 
+    1072         294 : void UavManager::timerFlightTime(const ros::TimerEvent& event) {
+    1073             : 
+    1074         294 :   if (!is_initialized_)
+    1075           0 :     return;
+    1076             : 
+    1077         882 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerFlightTime", _flighttime_timer_rate_, 0.1, event);
+    1078         882 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerFlightTime", scope_timer_logger_, scope_timer_enabled_);
+    1079             : 
+    1080         294 :   auto flighttime = mrs_lib::get_mutexed(mutex_flighttime_, flighttime_);
+    1081             : 
+    1082         294 :   flighttime += 1.0 / _flighttime_timer_rate_;
+    1083             : 
+    1084         294 :   mrs_msgs::Float64 flight_time;
+    1085         294 :   flight_time.value = flighttime;
+    1086             : 
+    1087             :   // if enabled, start the timer for measuring the flight time
+    1088         294 :   if (_flighttime_timer_enabled_) {
+    1089             : 
+    1090           0 :     if (flighttime > _flighttime_max_time_) {
+    1091             : 
+    1092           0 :       flighttime = 0;
+    1093           0 :       timer_flighttime_.stop();
+    1094             : 
+    1095           0 :       ROS_INFO("[UavManager]: max flight time reached, landing");
+    1096             : 
+    1097           0 :       landImpl();
+    1098             :     }
+    1099             :   }
+    1100             : 
+    1101         294 :   mrs_lib::set_mutexed(mutex_flighttime_, flighttime, flighttime_);
+    1102             : }
+    1103             : 
+    1104             : //}
+    1105             : 
+    1106             : /* //{ timerMaxthrottle() */
+    1107             : 
+    1108       58170 : void UavManager::timerMaxthrottle(const ros::TimerEvent& event) {
+    1109             : 
+    1110       58170 :   if (!is_initialized_)
+    1111       18010 :     return;
+    1112             : 
+    1113       58170 :   if (!sh_throttle_.hasMsg() || (ros::Time::now() - sh_throttle_.lastMsgTime()).toSec() > 1.0) {
+    1114       18010 :     return;
+    1115             :   }
+    1116             : 
+    1117       40160 :   ROS_INFO_ONCE("[UavManager]: max throttle timer spinning");
+    1118             : 
+    1119      120480 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerMaxthrottle", _maxthrottle_timer_rate_, 0.03, event);
+    1120      120480 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerMaxthrottle", scope_timer_logger_, scope_timer_enabled_);
+    1121             : 
+    1122       40160 :   auto desired_throttle = sh_throttle_.getMsg()->data;
+    1123             : 
+    1124       40160 :   if (desired_throttle >= _maxthrottle_max_throttle_) {
+    1125             : 
+    1126          32 :     if (!maxthrottle_above_threshold_) {
+    1127             : 
+    1128           1 :       maxthrottle_first_time_      = ros::Time::now();
+    1129           1 :       maxthrottle_above_threshold_ = true;
+    1130           1 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: max throttle exceeded threshold (%.2f/%.2f)", desired_throttle, _maxthrottle_max_throttle_);
+    1131             : 
+    1132             :     } else {
+    1133             : 
+    1134          31 :       ROS_WARN_THROTTLE(0.1, "[UavManager]: throttle over threshold (%.2f/%.2f) for %.2f s", desired_throttle, _maxthrottle_max_throttle_,
+    1135             :                         (ros::Time::now() - maxthrottle_first_time_).toSec());
+    1136             :     }
+    1137             : 
+    1138             :   } else {
+    1139             : 
+    1140       40128 :     maxthrottle_above_threshold_ = false;
+    1141             :   }
+    1142             : 
+    1143       40160 :   if (maxthrottle_above_threshold_ && (ros::Time::now() - maxthrottle_first_time_).toSec() > _maxthrottle_ungrip_timeout_) {
+    1144             : 
+    1145          16 :     ROS_WARN_THROTTLE(1.0, "[UavManager]: throttle over threshold (%.2f/%.2f) for more than %.2f s, ungripping payload", desired_throttle,
+    1146             :                       _maxthrottle_max_throttle_, _maxthrottle_ungrip_timeout_);
+    1147             : 
+    1148          16 :     ungripSrv();
+    1149             :   }
+    1150             : 
+    1151       40160 :   if (maxthrottle_above_threshold_ && (ros::Time::now() - maxthrottle_first_time_).toSec() > _maxthrottle_eland_timeout_) {
+    1152             : 
+    1153           1 :     timer_maxthrottle_.stop();
+    1154             : 
+    1155           1 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: throttle over threshold (%.2f/%.2f) for more than %.2f s, calling for emergency landing", desired_throttle,
+    1156             :                        _maxthrottle_max_throttle_, _maxthrottle_eland_timeout_);
+    1157             : 
+    1158           1 :     elandSrv();
+    1159             :   }
+    1160             : }
+    1161             : 
+    1162             : //}
+    1163             : 
+    1164             : /* //{ timerDiagnostics() */
+    1165             : 
+    1166        2259 : void UavManager::timerDiagnostics(const ros::TimerEvent& event) {
+    1167             : 
+    1168        2259 :   if (!is_initialized_)
+    1169           0 :     return;
+    1170             : 
+    1171        6777 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerDiagnostics", _maxthrottle_timer_rate_, 0.03, event);
+    1172        6777 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::timerDiagnostics", scope_timer_logger_, scope_timer_enabled_);
+    1173             : 
+    1174        2259 :   bool got_gps_est = false;
+    1175        2259 :   bool got_rtk_est = false;
+    1176             : 
+    1177        2259 :   if (sh_estimation_diagnostics_.hasMsg()) {  // get current position in lat-lon
+    1178             : 
+    1179        3760 :     auto                     estimation_diag  = sh_estimation_diagnostics_.getMsg();
+    1180        1880 :     std::vector<std::string> state_estimators = estimation_diag.get()->switchable_state_estimators;
+    1181             : 
+    1182        2552 :     got_gps_est = std::find(state_estimators.begin(), state_estimators.end(), "gps_garmin") != state_estimators.end() ||
+    1183         672 :                   std::find(state_estimators.begin(), state_estimators.end(), "gps_baro") != state_estimators.end();
+    1184        1880 :     got_rtk_est = std::find(state_estimators.begin(), state_estimators.end(), "rtk") != state_estimators.end();
+    1185             :   }
+    1186             : 
+    1187        4518 :   mrs_msgs::UavManagerDiagnostics diag;
+    1188             : 
+    1189        2259 :   diag.stamp    = ros::Time::now();
+    1190        2259 :   diag.uav_name = _uav_name_;
+    1191             : 
+    1192        2259 :   auto flighttime = mrs_lib::get_mutexed(mutex_flighttime_, flighttime_);
+    1193             : 
+    1194             :   // fill in the acumulated flight time
+    1195        2259 :   diag.flight_time = flighttime;
+    1196             : 
+    1197        2259 :   if (sh_odometry_.hasMsg()) {  // get current position in lat-lon
+    1198             : 
+    1199        1895 :     if (got_gps_est || got_rtk_est) {
+    1200             : 
+    1201        3702 :       nav_msgs::Odometry odom = *sh_odometry_.getMsg();
+    1202             : 
+    1203        3702 :       geometry_msgs::PoseStamped uav_pose;
+    1204        1851 :       uav_pose.pose = mrs_lib::getPose(odom);
+    1205             : 
+    1206        5553 :       auto res = transformer_->transformSingle(uav_pose, "latlon_origin");
+    1207             : 
+    1208        1851 :       if (res) {
+    1209        1851 :         diag.cur_latitude  = res.value().pose.position.x;
+    1210        1851 :         diag.cur_longitude = res.value().pose.position.y;
+    1211             :       }
+    1212             :     }
+    1213             :   }
+    1214             : 
+    1215        2259 :   if (takeoff_successful_) {
+    1216             : 
+    1217         339 :     if (got_gps_est || got_rtk_est) {
+    1218             : 
+    1219         678 :       auto land_there_reference = mrs_lib::get_mutexed(mutex_land_there_reference_, land_there_reference_);
+    1220             : 
+    1221        1017 :       auto res = transformer_->transformSingle(land_there_reference, "latlon_origin");
+    1222             : 
+    1223         339 :       if (res) {
+    1224         339 :         diag.home_latitude  = res.value().reference.position.x;
+    1225         339 :         diag.home_longitude = res.value().reference.position.y;
+    1226             :       }
+    1227             :     }
+    1228             :   }
+    1229             : 
+    1230        2259 :   ph_diag_.publish(diag);
+    1231             : }
+    1232             : 
+    1233             : //}
+    1234             : 
+    1235             : /* //{ timerMidairActivation() */
+    1236             : 
+    1237          80 : void UavManager::timerMidairActivation([[maybe_unused]] const ros::TimerEvent& event) {
+    1238             : 
+    1239          80 :   if (!is_initialized_)
+    1240           0 :     return;
+    1241             : 
+    1242          80 :   ROS_INFO_THROTTLE(0.1, "[UavManager]: waiting for OFFBOARD");
+    1243             : 
+    1244          80 :   if (sh_hw_api_status_.getMsg()->offboard) {
+    1245             : 
+    1246          80 :     ROS_INFO("[UavManager]: OFFBOARD detected");
+    1247             : 
+    1248          80 :     setOdometryCallbacksSrv(true);
+    1249             : 
+    1250             :     {
+    1251          80 :       bool controller_switched = switchControllerSrv(_midair_activation_after_controller_);
+    1252             : 
+    1253          80 :       if (!controller_switched) {
+    1254             : 
+    1255           1 :         ROS_ERROR("[UavManager]: could not activate '%s'", _midair_activation_after_controller_.c_str());
+    1256             : 
+    1257           1 :         ehoverSrv();
+    1258             : 
+    1259           1 :         timer_midair_activation_.stop();
+    1260             : 
+    1261           1 :         return;
+    1262             :       }
+    1263             :     }
+    1264             : 
+    1265             :     {
+    1266          79 :       bool tracker_switched = switchTrackerSrv(_midair_activation_after_tracker_);
+    1267             : 
+    1268          79 :       if (!tracker_switched) {
+    1269             : 
+    1270           1 :         ROS_ERROR("[UavManager]: could not activate '%s'", _midair_activation_after_tracker_.c_str());
+    1271             : 
+    1272           1 :         ehoverSrv();
+    1273             : 
+    1274           1 :         timer_midair_activation_.stop();
+    1275             : 
+    1276           1 :         return;
+    1277             :       }
+    1278             :     }
+    1279             : 
+    1280          78 :     timer_midair_activation_.stop();
+    1281             : 
+    1282          78 :     return;
+    1283             :   }
+    1284             : 
+    1285           0 :   if ((ros::Time::now() - midair_activation_started_).toSec() > 0.5) {
+    1286             : 
+    1287           0 :     ROS_ERROR("[UavManager]: waiting for OFFBOARD timeouted, reverting");
+    1288             : 
+    1289           0 :     toggleControlOutput(false);
+    1290             : 
+    1291           0 :     timer_midair_activation_.stop();
+    1292             : 
+    1293           0 :     return;
+    1294             :   }
+    1295             : }
+    1296             : 
+    1297             : //}
+    1298             : 
+    1299             : // --------------------------------------------------------------
+    1300             : // |                          callbacks                         |
+    1301             : // --------------------------------------------------------------
+    1302             : 
+    1303             : // | --------------------- topic callbacks -------------------- |
+    1304             : 
+    1305             : /* //{ callbackHwApiGNSS() */
+    1306             : 
+    1307       80568 : void UavManager::callbackHwApiGNSS(const sensor_msgs::NavSatFix::ConstPtr msg) {
+    1308             : 
+    1309       80568 :   if (!is_initialized_)
+    1310          25 :     return;
+    1311             : 
+    1312      241629 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackHwApiGNSS");
+    1313      241629 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("UavManager::callbackHwApiGNSS", scope_timer_logger_, scope_timer_enabled_);
+    1314             : 
+    1315       80543 :   transformer_->setLatLon(msg->latitude, msg->longitude);
+    1316             : }
+    1317             : 
+    1318             : //}
+    1319             : 
+    1320             : /* //{ callbackOdometry() */
+    1321             : 
+    1322      176140 : void UavManager::callbackOdometry(const nav_msgs::Odometry::ConstPtr msg) {
+    1323             : 
+    1324      176140 :   if (!is_initialized_)
+    1325           0 :     return;
+    1326             : 
+    1327      176140 :   transformer_->setDefaultFrame(msg->header.frame_id);
+    1328             : }
+    1329             : 
+    1330             : //}
+    1331             : 
+    1332             : // | -------------------- service callbacks ------------------- |
+    1333             : 
+    1334             : /* //{ callbackTakeoff() */
+    1335             : 
+    1336          23 : bool UavManager::callbackTakeoff([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1337             : 
+    1338          23 :   if (!is_initialized_)
+    1339           0 :     return false;
+    1340             : 
+    1341          23 :   ROS_INFO("[UavManager]: takeoff called by service");
+    1342             : 
+    1343             :   /* preconditions //{ */
+    1344             : 
+    1345             :   {
+    1346          23 :     std::stringstream ss;
+    1347             : 
+    1348          23 :     if (!sh_odometry_.hasMsg()) {
+    1349           0 :       ss << "can not takeoff, missing odometry!";
+    1350           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1351           0 :       res.message = ss.str();
+    1352           0 :       res.success = false;
+    1353           0 :       return true;
+    1354             :     }
+    1355             : 
+    1356          23 :     if (!sh_hw_api_status_.hasMsg() || (ros::Time::now() - sh_hw_api_status_.lastMsgTime()).toSec() > 5.0) {
+    1357           0 :       ss << "can not takeoff, missing HW API status!";
+    1358           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1359           0 :       res.message = ss.str();
+    1360           0 :       res.success = false;
+    1361           0 :       return true;
+    1362             :     }
+    1363             : 
+    1364          23 :     if (!sh_hw_api_status_.getMsg()->armed) {
+    1365           0 :       ss << "can not takeoff, UAV not armed!";
+    1366           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1367           0 :       res.message = ss.str();
+    1368           0 :       res.success = false;
+    1369           0 :       return true;
+    1370             :     }
+    1371             : 
+    1372          23 :     if (!sh_hw_api_status_.getMsg()->offboard) {
+    1373           0 :       ss << "can not takeoff, UAV not in offboard mode!";
+    1374           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1375           0 :       res.message = ss.str();
+    1376           0 :       res.success = false;
+    1377           0 :       return true;
+    1378             :     }
+    1379             : 
+    1380             :     {
+    1381          23 :       if (!sh_control_manager_diag_.hasMsg() && (ros::Time::now() - sh_control_manager_diag_.lastMsgTime()).toSec() > 5.0) {
+    1382           0 :         ss << "can not takeoff, missing control manager diagnostics!";
+    1383           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1384           0 :         res.message = ss.str();
+    1385           0 :         res.success = false;
+    1386           0 :         return true;
+    1387             :       }
+    1388             : 
+    1389          23 :       if (_null_tracker_name_ != sh_control_manager_diag_.getMsg()->active_tracker) {
+    1390           0 :         ss << "can not takeoff, need '" << _null_tracker_name_ << "' to be active!";
+    1391           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1392           0 :         res.message = ss.str();
+    1393           0 :         res.success = false;
+    1394           0 :         return true;
+    1395             :       }
+    1396             :     }
+    1397             : 
+    1398          23 :     if (!sh_controller_diagnostics_.hasMsg()) {
+    1399           0 :       ss << "can not takeoff, missing controller diagnostics!";
+    1400           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1401           0 :       res.message = ss.str();
+    1402           0 :       res.success = false;
+    1403           0 :       return true;
+    1404             :     }
+    1405             : 
+    1406          23 :     if (_gain_manager_required_ && (ros::Time::now() - sh_gains_diag_.lastMsgTime()).toSec() > 5.0) {
+    1407           0 :       ss << "can not takeoff, GainManager is not running!";
+    1408           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1409           0 :       res.message = ss.str();
+    1410           0 :       res.success = false;
+    1411           0 :       return true;
+    1412             :     }
+    1413             : 
+    1414          23 :     if (_constraint_manager_required_ && (ros::Time::now() - sh_constraints_diag_.lastMsgTime()).toSec() > 5.0) {
+    1415           0 :       ss << "can not takeoff, ConstraintManager is not running!";
+    1416           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1417           0 :       res.message = ss.str();
+    1418           0 :       res.success = false;
+    1419           0 :       return true;
+    1420             :     }
+    1421             : 
+    1422          23 :     if (!sh_control_manager_diag_.getMsg()->output_enabled) {
+    1423             : 
+    1424           0 :       ss << "can not takeoff, Control Manager's output is disabled!";
+    1425           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1426           0 :       res.message = ss.str();
+    1427           0 :       res.success = false;
+    1428           0 :       return true;
+    1429             :     }
+    1430             : 
+    1431          23 :     if (number_of_takeoffs_ > 0) {
+    1432             : 
+    1433           0 :       auto last_mass_difference = mrs_lib::get_mutexed(mutex_last_mass_difference_, last_mass_difference_);
+    1434             : 
+    1435           0 :       if (last_mass_difference > 1.0) {
+    1436             : 
+    1437           0 :         ss << std::setprecision(2);
+    1438           0 :         ss << "can not takeoff, estimated mass difference is too large: " << _null_tracker_name_ << "!";
+    1439           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1440           0 :         res.message = ss.str();
+    1441           0 :         res.success = false;
+    1442           0 :         return true;
+    1443             :       }
+    1444             :     }
+    1445             :   }
+    1446             : 
+    1447             :   //}
+    1448             : 
+    1449          46 :   auto control_manager_diagnostics = sh_control_manager_diag_.getMsg();
+    1450          46 :   auto odometry                    = sh_odometry_.getMsg();
+    1451          23 :   auto [odom_x, odom_y, odom_z]    = mrs_lib::getPosition(sh_odometry_.getMsg());
+    1452             : 
+    1453             :   double odom_heading;
+    1454             :   try {
+    1455          23 :     odom_heading = mrs_lib::getHeading(sh_odometry_.getMsg());
+    1456             :   }
+    1457           0 :   catch (mrs_lib::AttitudeConverter::GetHeadingException& e) {
+    1458           0 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: exception caught: '%s'", e.what());
+    1459             : 
+    1460           0 :     std::stringstream ss;
+    1461           0 :     ss << "could not calculate current heading";
+    1462           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1463           0 :     res.message = ss.str();
+    1464           0 :     res.success = false;
+    1465           0 :     return true;
+    1466             :   }
+    1467             : 
+    1468          23 :   ROS_INFO("[UavManager]: taking off");
+    1469             : 
+    1470          23 :   setOdometryCallbacksSrv(false);
+    1471             : 
+    1472             :   // activating the takeoff controller
+    1473             :   {
+    1474          23 :     std::string old_controller      = sh_control_manager_diag_.getMsg()->active_controller;
+    1475          23 :     bool        controller_switched = switchControllerSrv(_takeoff_controller_name_);
+    1476             : 
+    1477             :     // if it fails, activate back the old controller
+    1478             :     // this is no big deal since the control outputs are not used
+    1479             :     // until NullTracker is active
+    1480          23 :     if (!controller_switched) {
+    1481             : 
+    1482           1 :       std::stringstream ss;
+    1483           1 :       ss << "could not activate '" << _takeoff_controller_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             :   // activate the takeoff tracker
+    1496             :   {
+    1497          22 :     std::string old_tracker      = sh_control_manager_diag_.getMsg()->active_tracker;
+    1498          22 :     bool        tracker_switched = switchTrackerSrv(_takeoff_tracker_name_);
+    1499             : 
+    1500             :     // if it fails, activate back the old tracker
+    1501          22 :     if (!tracker_switched) {
+    1502             : 
+    1503           1 :       std::stringstream ss;
+    1504           1 :       ss << "could not activate '" << _takeoff_tracker_name_ << "' for takeoff";
+    1505           1 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1506           1 :       res.success = false;
+    1507           1 :       res.message = ss.str();
+    1508             : 
+    1509           1 :       toggleControlOutput(false);
+    1510           1 :       disarmSrv();
+    1511             : 
+    1512           1 :       return true;
+    1513             :     }
+    1514             :   }
+    1515             : 
+    1516             :   // let's sleep before calling take off.. the motors are rumping-up anyway
+    1517             :   // this solves on-time-happening race condition in the landoff tracker
+    1518          21 :   ros::Duration(0.3).sleep();
+    1519             : 
+    1520             :   // now the takeoff tracker and controller are active
+    1521             :   // the UAV is basically hovering on the ground
+    1522             :   // (the controller is probably rumping up the throttle now)
+    1523             : 
+    1524             :   // call the takeoff service at the takeoff tracker
+    1525             :   {
+    1526          21 :     bool takeoff_successful = takeoffSrv();
+    1527             : 
+    1528             :     // if the takeoff was not successful, switch to NullTracker
+    1529          21 :     if (takeoff_successful) {
+    1530             : 
+    1531             :       // save the current spot for later landing
+    1532             :       {
+    1533          21 :         std::scoped_lock lock(mutex_land_there_reference_);
+    1534             : 
+    1535          21 :         land_there_reference_.header               = odometry->header;
+    1536          21 :         land_there_reference_.reference.position.x = odom_x;
+    1537          21 :         land_there_reference_.reference.position.y = odom_y;
+    1538          21 :         land_there_reference_.reference.position.z = odom_z;
+    1539          21 :         land_there_reference_.reference.heading    = odom_heading;
+    1540             :       }
+    1541             : 
+    1542          21 :       timer_flighttime_.start();
+    1543             : 
+    1544          42 :       std::stringstream ss;
+    1545          21 :       ss << "taking off";
+    1546          21 :       res.success = true;
+    1547          21 :       res.message = ss.str();
+    1548          21 :       ROS_INFO_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1549             : 
+    1550          21 :       takingoff_ = true;
+    1551          21 :       number_of_takeoffs_++;
+    1552          21 :       waiting_for_takeoff_ = true;
+    1553             : 
+    1554             :       // start the takeoff timer
+    1555          21 :       timer_takeoff_.start();
+    1556             : 
+    1557          21 :       takeoff_successful_ = takeoff_successful;
+    1558             : 
+    1559             :     } else {
+    1560             : 
+    1561           0 :       std::stringstream ss;
+    1562           0 :       ss << "takeoff was not successful";
+    1563           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1564           0 :       res.success = false;
+    1565           0 :       res.message = ss.str();
+    1566             : 
+    1567             :       // if the call for takeoff fails, call for emergency landing
+    1568           0 :       elandSrv();
+    1569             :     }
+    1570             :   }
+    1571             : 
+    1572          21 :   return true;
+    1573             : }
+    1574             : 
+    1575             : //}
+    1576             : 
+    1577             : /* //{ callbackLand() */
+    1578             : 
+    1579           2 : bool UavManager::callbackLand([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1580             : 
+    1581           2 :   if (!is_initialized_)
+    1582           0 :     return false;
+    1583             : 
+    1584           2 :   ROS_INFO("[UavManager]: land called by service");
+    1585             : 
+    1586             :   /* preconditions //{ */
+    1587             : 
+    1588             :   {
+    1589           2 :     std::stringstream ss;
+    1590             : 
+    1591           2 :     if (!sh_odometry_.hasMsg()) {
+    1592           0 :       ss << "can not land, missing odometry!";
+    1593           0 :       res.message = ss.str();
+    1594           0 :       res.success = false;
+    1595           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1596           0 :       return true;
+    1597             :     }
+    1598             : 
+    1599             :     {
+    1600           2 :       if (!sh_control_manager_diag_.hasMsg()) {
+    1601           0 :         ss << "can not land, missing control manager diagnostics!";
+    1602           0 :         res.message = ss.str();
+    1603           0 :         res.success = false;
+    1604           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1605           0 :         return true;
+    1606             :       }
+    1607             : 
+    1608           2 :       if (_null_tracker_name_ == sh_control_manager_diag_.getMsg()->active_tracker) {
+    1609           0 :         ss << "can not land, '" << _null_tracker_name_ << "' is active!";
+    1610           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1611           0 :         res.message = ss.str();
+    1612           0 :         res.success = false;
+    1613           0 :         return true;
+    1614             :       }
+    1615             :     }
+    1616             : 
+    1617           2 :     if (!sh_controller_diagnostics_.hasMsg()) {
+    1618           0 :       ss << "can not land, missing controller diagnostics!";
+    1619           0 :       res.message = ss.str();
+    1620           0 :       res.success = false;
+    1621           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1622           0 :       return true;
+    1623             :     }
+    1624             : 
+    1625           2 :     if (!sh_tracker_cmd_.hasMsg()) {
+    1626           0 :       ss << "can not land, missing position cmd!";
+    1627           0 :       res.message = ss.str();
+    1628           0 :       res.success = false;
+    1629           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1630           0 :       return true;
+    1631             :     }
+    1632             :   }
+    1633             : 
+    1634             :   //}
+    1635             : 
+    1636           2 :   auto [success, message] = landWithDescendImpl();
+    1637             : 
+    1638           2 :   res.message = message;
+    1639           2 :   res.success = success;
+    1640             : 
+    1641           2 :   return true;
+    1642             : }
+    1643             : 
+    1644             : //}
+    1645             : 
+    1646             : /* //{ callbackLandHome() */
+    1647             : 
+    1648           2 : bool UavManager::callbackLandHome([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1649             : 
+    1650           2 :   if (!is_initialized_)
+    1651           0 :     return false;
+    1652             : 
+    1653           2 :   ROS_INFO("[UavManager]: land home called by service");
+    1654             : 
+    1655             :   /* preconditions //{ */
+    1656             : 
+    1657             :   {
+    1658           2 :     std::stringstream ss;
+    1659             : 
+    1660           2 :     if (number_of_takeoffs_ == 0) {
+    1661           0 :       ss << "can not land home, did not takeoff before!";
+    1662           0 :       res.message = ss.str();
+    1663           0 :       res.success = false;
+    1664           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1665           0 :       return true;
+    1666             :     }
+    1667             : 
+    1668           2 :     if (!sh_odometry_.hasMsg()) {
+    1669           0 :       ss << "can not land, missing odometry!";
+    1670           0 :       res.message = ss.str();
+    1671           0 :       res.success = false;
+    1672           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1673           0 :       return true;
+    1674             :     }
+    1675             : 
+    1676             :     {
+    1677           2 :       if (!sh_control_manager_diag_.hasMsg()) {
+    1678           0 :         ss << "can not land, missing tracker status!";
+    1679           0 :         res.message = ss.str();
+    1680           0 :         res.success = false;
+    1681           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1682           0 :         return true;
+    1683             :       }
+    1684             : 
+    1685           2 :       if (_null_tracker_name_ == sh_control_manager_diag_.getMsg()->active_tracker) {
+    1686           0 :         ss << "can not land, '" << _null_tracker_name_ << "' is active!";
+    1687           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1688           0 :         res.message = ss.str();
+    1689           0 :         res.success = false;
+    1690           0 :         return true;
+    1691             :       }
+    1692             :     }
+    1693             : 
+    1694           2 :     if (!sh_controller_diagnostics_.hasMsg()) {
+    1695           0 :       ss << "can not land, missing controller diagnostics command!";
+    1696           0 :       res.message = ss.str();
+    1697           0 :       res.success = false;
+    1698           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1699           0 :       return true;
+    1700             :     }
+    1701             : 
+    1702           2 :     if (!sh_tracker_cmd_.hasMsg()) {
+    1703           0 :       ss << "can not land, missing position cmd!";
+    1704           0 :       res.message = ss.str();
+    1705           0 :       res.success = false;
+    1706           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1707           0 :       return true;
+    1708             :     }
+    1709             : 
+    1710           2 :     if (fixing_max_height_) {
+    1711           0 :       ss << "can not land, descedning to safe height!";
+    1712           0 :       res.message = ss.str();
+    1713           0 :       res.success = false;
+    1714           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1715           0 :       return true;
+    1716             :     }
+    1717             : 
+    1718           2 :     if (current_state_landing_ != IDLE_STATE) {
+    1719           0 :       ss << "can not land, already landing!";
+    1720           0 :       res.message = ss.str();
+    1721           0 :       res.success = false;
+    1722           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1723           0 :       return true;
+    1724             :     }
+    1725             :   }
+    1726             : 
+    1727             :   //}
+    1728             : 
+    1729           2 :   ungripSrv();
+    1730             : 
+    1731           4 :   mrs_msgs::ReferenceStamped reference_out;
+    1732             : 
+    1733             :   {
+    1734           2 :     std::scoped_lock lock(mutex_land_there_reference_);
+    1735             : 
+    1736             :     // get the current altitude in land_there_reference_.header.frame_id;
+    1737           2 :     geometry_msgs::PoseStamped current_pose;
+    1738           2 :     current_pose.header.stamp     = ros::Time::now();
+    1739           2 :     current_pose.header.frame_id  = _uav_name_ + "/fcu";
+    1740           2 :     current_pose.pose.position.x  = 0;
+    1741           2 :     current_pose.pose.position.y  = 0;
+    1742           2 :     current_pose.pose.position.z  = 0;
+    1743           2 :     current_pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    1744             : 
+    1745           2 :     auto response = transformer_->transformSingle(current_pose, land_there_reference_.header.frame_id);
+    1746             : 
+    1747           2 :     if (response) {
+    1748             : 
+    1749           2 :       land_there_reference_.reference.position.z = response.value().pose.position.z;
+    1750           2 :       ROS_DEBUG("[UavManager]: current altitude is %.2f m", land_there_reference_.reference.position.z);
+    1751             : 
+    1752             :     } else {
+    1753             : 
+    1754           0 :       std::stringstream ss;
+    1755           0 :       ss << "could not transform current height to " << land_there_reference_.header.frame_id;
+    1756           0 :       ROS_ERROR_STREAM("[UavManager]: " << ss.str());
+    1757             : 
+    1758           0 :       res.success = false;
+    1759           0 :       res.message = ss.str();
+    1760           0 :       return true;
+    1761             :     }
+    1762             : 
+    1763           2 :     reference_out.header.frame_id = land_there_reference_.header.frame_id;
+    1764           2 :     reference_out.header.stamp    = ros::Time::now();
+    1765           2 :     reference_out.reference       = land_there_reference_.reference;
+    1766             : 
+    1767           2 :     land_there_reference_ = reference_out;
+    1768             :   }
+    1769             : 
+    1770           2 :   bool service_success = emergencyReferenceSrv(reference_out);
+    1771             : 
+    1772           2 :   if (service_success) {
+    1773             : 
+    1774           4 :     std::stringstream ss;
+    1775           2 :     ss << "flying home for landing";
+    1776           2 :     ROS_INFO_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1777             : 
+    1778           2 :     res.success = true;
+    1779           2 :     res.message = ss.str();
+    1780             : 
+    1781             :     // stop the eventual takeoff
+    1782           2 :     waiting_for_takeoff_ = false;
+    1783           2 :     takingoff_           = false;
+    1784           2 :     timer_takeoff_.stop();
+    1785             : 
+    1786           2 :     throttle_under_threshold_          = false;
+    1787           2 :     throttle_mass_estimate_first_time_ = ros::Time(0);
+    1788             : 
+    1789           2 :     changeLandingState(GOTO_STATE);
+    1790             : 
+    1791           2 :     timer_landing_.start();
+    1792             : 
+    1793             :   } else {
+    1794             : 
+    1795           0 :     std::stringstream ss;
+    1796           0 :     ss << "can not fly home for landing";
+    1797           0 :     ROS_ERROR_STREAM("[UavManager]: " << ss.str());
+    1798             : 
+    1799           0 :     res.success = false;
+    1800           0 :     res.message = ss.str();
+    1801             :   }
+    1802             : 
+    1803           2 :   return true;
+    1804             : }
+    1805             : 
+    1806             : //}
+    1807             : 
+    1808             : /* //{ callbackLandThere() */
+    1809             : 
+    1810           1 : bool UavManager::callbackLandThere(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res) {
+    1811             : 
+    1812           1 :   if (!is_initialized_)
+    1813           0 :     return false;
+    1814             : 
+    1815           1 :   ROS_INFO("[UavManager]: land there called by service");
+    1816             : 
+    1817             :   /* preconditions //{ */
+    1818             : 
+    1819             :   {
+    1820           1 :     std::stringstream ss;
+    1821             : 
+    1822           1 :     if (!sh_odometry_.hasMsg()) {
+    1823           0 :       ss << "can not land, missing odometry!";
+    1824           0 :       res.message = ss.str();
+    1825           0 :       res.success = false;
+    1826           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1827           0 :       return true;
+    1828             :     }
+    1829             : 
+    1830             :     {
+    1831           1 :       if (!sh_control_manager_diag_.hasMsg()) {
+    1832           0 :         ss << "can not land, missing tracker status!";
+    1833           0 :         res.message = ss.str();
+    1834           0 :         res.success = false;
+    1835           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1836           0 :         return true;
+    1837             :       }
+    1838             : 
+    1839           1 :       if (_null_tracker_name_ == sh_control_manager_diag_.getMsg()->active_tracker) {
+    1840           0 :         ss << "can not land, '" << _null_tracker_name_ << "' is active!";
+    1841           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1842           0 :         res.message = ss.str();
+    1843           0 :         res.success = false;
+    1844           0 :         return true;
+    1845             :       }
+    1846             :     }
+    1847             : 
+    1848           1 :     if (!sh_controller_diagnostics_.hasMsg()) {
+    1849           0 :       ss << "can not land, missing controller diagnostics!";
+    1850           0 :       res.message = ss.str();
+    1851           0 :       res.success = false;
+    1852           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1853           0 :       return true;
+    1854             :     }
+    1855             : 
+    1856           1 :     if (!sh_tracker_cmd_.hasMsg()) {
+    1857           0 :       ss << "can not land, missing position cmd!";
+    1858           0 :       res.message = ss.str();
+    1859           0 :       res.success = false;
+    1860           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1861           0 :       return true;
+    1862             :     }
+    1863             : 
+    1864           1 :     if (fixing_max_height_) {
+    1865           0 :       ss << "can not land, descedning to safe height!";
+    1866           0 :       res.message = ss.str();
+    1867           0 :       res.success = false;
+    1868           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1869           0 :       return true;
+    1870             :     }
+    1871             :   }
+    1872             : 
+    1873             :   //}
+    1874             : 
+    1875           1 :   ungripSrv();
+    1876             : 
+    1877           2 :   auto odometry = sh_odometry_.getMsg();
+    1878             : 
+    1879             :   // | ------ transform the reference to the current frame ------ |
+    1880             : 
+    1881           2 :   mrs_msgs::ReferenceStamped reference_in;
+    1882           1 :   reference_in.header    = req.header;
+    1883           1 :   reference_in.reference = req.reference;
+    1884             : 
+    1885           2 :   auto result = transformer_->transformSingle(reference_in, odometry->header.frame_id);
+    1886             : 
+    1887           1 :   if (!result) {
+    1888           0 :     std::stringstream ss;
+    1889           0 :     ss << "can not transform the reference to the current control frame!";
+    1890           0 :     res.message = ss.str();
+    1891           0 :     res.success = false;
+    1892           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1893           0 :     return true;
+    1894             :   }
+    1895             : 
+    1896             :   {
+    1897           1 :     std::scoped_lock lock(mutex_land_there_reference_);
+    1898             : 
+    1899           1 :     land_there_reference_.header               = odometry->header;
+    1900           1 :     land_there_reference_.reference            = reference_in.reference;
+    1901           1 :     land_there_reference_.reference.position.z = odometry->pose.pose.position.z;
+    1902             :   }
+    1903             : 
+    1904           1 :   bool service_success = emergencyReferenceSrv(land_there_reference_);
+    1905             : 
+    1906           1 :   if (service_success) {
+    1907             : 
+    1908           2 :     std::stringstream ss;
+    1909           1 :     ss << "flying there for landing";
+    1910           1 :     ROS_INFO_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1911             : 
+    1912           1 :     res.success = true;
+    1913           1 :     res.message = ss.str();
+    1914             : 
+    1915             :     // stop the eventual takeoff
+    1916           1 :     waiting_for_takeoff_ = false;
+    1917           1 :     takingoff_           = false;
+    1918           1 :     timer_takeoff_.stop();
+    1919             : 
+    1920           1 :     throttle_under_threshold_          = false;
+    1921           1 :     throttle_mass_estimate_first_time_ = ros::Time(0);
+    1922             : 
+    1923           1 :     changeLandingState(GOTO_STATE);
+    1924             : 
+    1925           1 :     timer_landing_.start();
+    1926             : 
+    1927             :   } else {
+    1928             : 
+    1929           0 :     std::stringstream ss;
+    1930           0 :     ss << "can not fly there for landing";
+    1931           0 :     ROS_ERROR_STREAM("[UavManager]: " << ss.str());
+    1932             : 
+    1933           0 :     res.success = false;
+    1934           0 :     res.message = ss.str();
+    1935             :   }
+    1936             : 
+    1937           1 :   return true;
+    1938             : }
+    1939             : 
+    1940             : //}
+    1941             : 
+    1942             : /* //{ callbackMidairActivation() */
+    1943             : 
+    1944          82 : bool UavManager::callbackMidairActivation([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1945             : 
+    1946          82 :   if (!is_initialized_)
+    1947           0 :     return false;
+    1948             : 
+    1949          82 :   ROS_INFO("[UavManager]: midair activation called by service");
+    1950             : 
+    1951             :   /* preconditions //{ */
+    1952             : 
+    1953             :   {
+    1954          82 :     std::stringstream ss;
+    1955             : 
+    1956          82 :     if (!sh_odometry_.hasMsg()) {
+    1957           0 :       ss << "can not activate, missing odometry!";
+    1958           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1959           0 :       res.message = ss.str();
+    1960           0 :       res.success = false;
+    1961           0 :       return true;
+    1962             :     }
+    1963             : 
+    1964          82 :     if (!sh_hw_api_status_.hasMsg() || (ros::Time::now() - sh_hw_api_status_.lastMsgTime()).toSec() > 5.0) {
+    1965           0 :       ss << "can not activate, missing HW API status!";
+    1966           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1967           0 :       res.message = ss.str();
+    1968           0 :       res.success = false;
+    1969           0 :       return true;
+    1970             :     }
+    1971             : 
+    1972          82 :     if (!sh_hw_api_status_.getMsg()->armed) {
+    1973           0 :       ss << "can not activate, UAV not armed!";
+    1974           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1975           0 :       res.message = ss.str();
+    1976           0 :       res.success = false;
+    1977           0 :       return true;
+    1978             :     }
+    1979             : 
+    1980          82 :     if (sh_hw_api_status_.getMsg()->offboard) {
+    1981           0 :       ss << "can not activate, UAV already in offboard mode!";
+    1982           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1983           0 :       res.message = ss.str();
+    1984           0 :       res.success = false;
+    1985           0 :       return true;
+    1986             :     }
+    1987             : 
+    1988             :     {
+    1989          82 :       if (!sh_control_manager_diag_.hasMsg()) {
+    1990           0 :         ss << "can not activate, missing control manager diagnostics!";
+    1991           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    1992           0 :         res.message = ss.str();
+    1993           0 :         res.success = false;
+    1994           0 :         return true;
+    1995             :       }
+    1996             : 
+    1997          82 :       if (_null_tracker_name_ != sh_control_manager_diag_.getMsg()->active_tracker) {
+    1998           0 :         ss << "can not activate, need '" << _null_tracker_name_ << "' to be active!";
+    1999           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2000           0 :         res.message = ss.str();
+    2001           0 :         res.success = false;
+    2002           0 :         return true;
+    2003             :       }
+    2004             :     }
+    2005             : 
+    2006          82 :     if (!sh_controller_diagnostics_.hasMsg()) {
+    2007           0 :       ss << "can not activate, missing controller diagnostics!";
+    2008           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2009           0 :       res.message = ss.str();
+    2010           0 :       res.success = false;
+    2011           0 :       return true;
+    2012             :     }
+    2013             : 
+    2014          82 :     if (_gain_manager_required_ && (ros::Time::now() - sh_gains_diag_.lastMsgTime()).toSec() > 5.0) {
+    2015           0 :       ss << "can not activate, GainManager is not running!";
+    2016           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2017           0 :       res.message = ss.str();
+    2018           0 :       res.success = false;
+    2019           0 :       return true;
+    2020             :     }
+    2021             : 
+    2022          82 :     if (_constraint_manager_required_ && (ros::Time::now() - sh_constraints_diag_.lastMsgTime()).toSec() > 5.0) {
+    2023           0 :       ss << "can not activate, ConstraintManager is not running!";
+    2024           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2025           0 :       res.message = ss.str();
+    2026           0 :       res.success = false;
+    2027           0 :       return true;
+    2028             :     }
+    2029             : 
+    2030          82 :     if (number_of_takeoffs_ > 0) {
+    2031           0 :       ss << "can not activate, we flew already!";
+    2032           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2033           0 :       res.message = ss.str();
+    2034           0 :       res.success = false;
+    2035           0 :       return true;
+    2036             :     }
+    2037             :   }
+    2038             : 
+    2039             :   //}
+    2040             : 
+    2041          82 :   auto [success, message] = midairActivationImpl();
+    2042             : 
+    2043          82 :   res.message = message;
+    2044          82 :   res.success = success;
+    2045             : 
+    2046          82 :   return true;
+    2047             : }
+    2048             : 
+    2049             : //}
+    2050             : 
+    2051             : /* //{ callbackMinHeightCheck() */
+    2052             : 
+    2053           2 : bool UavManager::callbackMinHeightCheck([[maybe_unused]] std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    2054             : 
+    2055           2 :   if (!is_initialized_)
+    2056           0 :     return false;
+    2057             : 
+    2058           2 :   min_height_check_ = req.data;
+    2059             : 
+    2060           2 :   std::stringstream ss;
+    2061             : 
+    2062           2 :   ss << "min height check " << (min_height_check_ ? "enabled" : "disabled");
+    2063             : 
+    2064           2 :   if (min_height_check_) {
+    2065           1 :     timer_min_height_.start();
+    2066             :   } else {
+    2067           1 :     timer_min_height_.stop();
+    2068             :   }
+    2069             : 
+    2070           2 :   ROS_INFO_STREAM("[UavManager]: " << ss.str());
+    2071             : 
+    2072           2 :   res.message = ss.str();
+    2073           2 :   res.success = true;
+    2074             : 
+    2075           2 :   return true;
+    2076             : }
+    2077             : 
+    2078             : //}
+    2079             : 
+    2080             : // | ------------------------ routines ------------------------ |
+    2081             : 
+    2082             : /* landImpl() //{ */
+    2083             : 
+    2084           5 : std::tuple<bool, std::string> UavManager::landImpl(void) {
+    2085             : 
+    2086             :   // activating the landing controller
+    2087             :   {
+    2088           5 :     std::string old_controller      = sh_control_manager_diag_.getMsg()->active_controller;
+    2089           5 :     bool        controller_switched = switchControllerSrv(_landing_controller_name_);
+    2090             : 
+    2091             :     // if it fails, activate eland
+    2092             :     // Tomas: I pressume that its more important to get the UAV to the ground rather than
+    2093             :     // just throw out error.
+    2094           5 :     if (!controller_switched) {
+    2095             : 
+    2096           0 :       std::stringstream ss;
+    2097           0 :       ss << "could not activate '" << _takeoff_controller_name_ << "' for landing";
+    2098           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2099             : 
+    2100           0 :       elandSrv();
+    2101             : 
+    2102           0 :       return std::tuple(false, ss.str());
+    2103             :     }
+    2104             :   }
+    2105             : 
+    2106             :   // activate the landing tracker
+    2107             :   {
+    2108           5 :     std::string old_tracker      = sh_control_manager_diag_.getMsg()->active_tracker;
+    2109           5 :     bool        tracker_switched = switchTrackerSrv(_landing_tracker_name_);
+    2110             : 
+    2111             :     // if it fails, activate eland
+    2112             :     // Tomas: I pressume that its more important to get the UAV to the ground rather than
+    2113             :     // just throw out error.
+    2114           5 :     if (!tracker_switched) {
+    2115             : 
+    2116           0 :       std::stringstream ss;
+    2117           0 :       ss << "could not activate '" << _takeoff_tracker_name_ << "' for landing";
+    2118           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2119             : 
+    2120           0 :       elandSrv();
+    2121             : 
+    2122           0 :       return std::tuple(false, ss.str());
+    2123             :     }
+    2124             :   }
+    2125             : 
+    2126             :   // call the landing service
+    2127             :   {
+    2128           5 :     bool land_successful = landSrv();
+    2129             : 
+    2130           5 :     if (land_successful) {
+    2131             : 
+    2132             :       // stop the eventual takeoff
+    2133           5 :       waiting_for_takeoff_ = false;
+    2134           5 :       takingoff_           = false;
+    2135           5 :       timer_takeoff_.stop();
+    2136             : 
+    2137             :       // stop counting the flight time
+    2138           5 :       timer_flighttime_.stop();
+    2139             : 
+    2140          10 :       auto controller_diagnostics = sh_controller_diagnostics_.getMsg();
+    2141             : 
+    2142             :       // remember the last valid mass estimated
+    2143             :       // used during subsequent takeoff
+    2144           5 :       if (controller_diagnostics->mass_estimator) {
+    2145           5 :         last_mass_difference_ = controller_diagnostics->mass_difference;
+    2146             :       }
+    2147             : 
+    2148           5 :       setOdometryCallbacksSrv(false);
+    2149             : 
+    2150           5 :       changeLandingState(LANDING_STATE);
+    2151             : 
+    2152           5 :       throttle_under_threshold_          = false;
+    2153           5 :       throttle_mass_estimate_first_time_ = ros::Time(0);
+    2154             : 
+    2155           5 :       timer_landing_.start();
+    2156             : 
+    2157           5 :       std::stringstream ss;
+    2158           5 :       ss << "landing initiated";
+    2159           5 :       ROS_INFO_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2160             : 
+    2161           5 :       return std::tuple(true, ss.str());
+    2162             : 
+    2163             :     } else {
+    2164             : 
+    2165           0 :       std::stringstream ss;
+    2166           0 :       ss << "could not land";
+    2167           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2168             : 
+    2169           0 :       elandSrv();
+    2170             : 
+    2171           0 :       return std::tuple(false, ss.str());
+    2172             :     }
+    2173             :   }
+    2174             : }
+    2175             : 
+    2176             : //}
+    2177             : 
+    2178             : /* landWithDescendImpl() //{ */
+    2179             : 
+    2180           9 : std::tuple<bool, std::string> UavManager::landWithDescendImpl(void) {
+    2181             : 
+    2182             :   // if the height information is available
+    2183           9 :   if (sh_height_.hasMsg()) {
+    2184             : 
+    2185           9 :     double height = sh_height_.getMsg()->value;
+    2186             : 
+    2187           9 :     if (height > 0 && height >= _landing_descend_height_ + 1.0) {
+    2188             : 
+    2189           4 :       auto odometry = sh_odometry_.getMsg();
+    2190             : 
+    2191           4 :       ungripSrv();
+    2192             : 
+    2193             :       {
+    2194           4 :         std::scoped_lock lock(mutex_land_there_reference_);
+    2195             : 
+    2196             :         // FOR FUTURE ME: Do not change this, we need it to be filled for the final check later
+    2197           4 :         land_there_reference_.header.frame_id      = "";
+    2198           4 :         land_there_reference_.header.stamp         = ros::Time::now();
+    2199           4 :         land_there_reference_.reference.position.x = odometry->pose.pose.position.x;
+    2200           4 :         land_there_reference_.reference.position.y = odometry->pose.pose.position.y;
+    2201           4 :         land_there_reference_.reference.position.z = odometry->pose.pose.position.z - (height - _landing_descend_height_);
+    2202           4 :         land_there_reference_.reference.heading    = mrs_lib::AttitudeConverter(odometry->pose.pose.orientation).getHeading();
+    2203             :       }
+    2204             : 
+    2205           4 :       bool service_success = emergencyReferenceSrv(land_there_reference_);
+    2206             : 
+    2207           4 :       if (service_success) {
+    2208             : 
+    2209           4 :         std::stringstream ss;
+    2210           4 :         ss << "flying down for landing";
+    2211           4 :         ROS_INFO_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2212             : 
+    2213             :         // stop the eventual takeoff
+    2214           4 :         waiting_for_takeoff_ = false;
+    2215           4 :         takingoff_           = false;
+    2216           4 :         timer_takeoff_.stop();
+    2217             : 
+    2218           4 :         changeLandingState(GOTO_STATE);
+    2219             : 
+    2220           4 :         throttle_under_threshold_          = false;
+    2221           4 :         throttle_mass_estimate_first_time_ = ros::Time(0);
+    2222             : 
+    2223           4 :         timer_landing_.start();
+    2224             : 
+    2225           4 :         return std::tuple(true, ss.str());
+    2226             : 
+    2227             :       } else {
+    2228             : 
+    2229           0 :         std::stringstream ss;
+    2230           0 :         ss << "can not fly down for landing";
+    2231           0 :         ROS_ERROR_STREAM("[UavManager]: " << ss.str());
+    2232             :       }
+    2233             :     }
+    2234             :   }
+    2235             : 
+    2236          10 :   auto [success, message] = landImpl();
+    2237             : 
+    2238           5 :   return std::tuple(success, message);
+    2239             : }
+    2240             : 
+    2241             : //}
+    2242             : 
+    2243             : /* midairActivationImpl() //{ */
+    2244             : 
+    2245          82 : std::tuple<bool, std::string> UavManager::midairActivationImpl(void) {
+    2246             : 
+    2247             :   // 1. activate the mid-air activation controller
+    2248             :   // the controller will output hover-like control output
+    2249         164 :   std::string old_controller;
+    2250             :   {
+    2251          82 :     old_controller           = sh_control_manager_diag_.getMsg()->active_controller;
+    2252          82 :     bool controller_switched = switchControllerSrv(_midair_activation_during_controller_);
+    2253             : 
+    2254          82 :     if (!controller_switched) {
+    2255             : 
+    2256           0 :       std::stringstream ss;
+    2257           0 :       ss << "could not activate '" << _midair_activation_during_controller_ << "' for midair activation";
+    2258           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2259             : 
+    2260           0 :       return std::tuple(false, ss.str());
+    2261             :     }
+    2262             :   }
+    2263             : 
+    2264             :   // 2. turn Control Manager's output ON
+    2265             :   {
+    2266          82 :     bool output_enabled = toggleControlOutput(true);
+    2267             : 
+    2268          82 :     if (!output_enabled) {
+    2269             : 
+    2270           1 :       switchControllerSrv(old_controller);
+    2271             : 
+    2272           1 :       std::stringstream ss;
+    2273           1 :       ss << "could not enable Control Manager's output";
+    2274           1 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2275             : 
+    2276           1 :       return std::tuple(false, ss.str());
+    2277             :     }
+    2278             :   }
+    2279             : 
+    2280             :   // 3. activate the mid-air activation tracker
+    2281             :   // this will cause the Control Manager to output something else than min-throttle
+    2282         162 :   std::string old_tracker;
+    2283             :   {
+    2284          81 :     old_tracker = sh_control_manager_diag_.getMsg()->active_tracker;
+    2285             : 
+    2286          81 :     bool tracker_switched = switchTrackerSrv(_midair_activation_during_tracker_);
+    2287             : 
+    2288          81 :     if (!tracker_switched) {
+    2289             : 
+    2290           0 :       switchControllerSrv(old_controller);
+    2291           0 :       toggleControlOutput(false);
+    2292             : 
+    2293           0 :       std::stringstream ss;
+    2294           0 :       ss << "could not activate '" << _midair_activation_during_tracker_ << "' for midair activation";
+    2295           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2296             : 
+    2297           0 :       return std::tuple(false, ss.str());
+    2298             :     }
+    2299             :   }
+    2300             : 
+    2301             :   // 4. wait for 50 ms, that should be enough for the Pixhawk to start getting data
+    2302          81 :   ros::Duration(0.05).sleep();
+    2303             : 
+    2304             :   // 5. turn on the OFFBOARD MODE
+    2305             :   // since now, the UAV should be under our control
+    2306             :   {
+    2307          81 :     bool offboard_set = offboardSrv(true);
+    2308             : 
+    2309          81 :     if (!offboard_set) {
+    2310             : 
+    2311           1 :       switchTrackerSrv(old_tracker);
+    2312           1 :       switchControllerSrv(old_controller);
+    2313           1 :       toggleControlOutput(false);
+    2314             : 
+    2315           1 :       std::stringstream ss;
+    2316           1 :       ss << "could not activate offboard mode";
+    2317           1 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2318             : 
+    2319           1 :       return std::tuple(false, ss.str());
+    2320             :     }
+    2321             :   }
+    2322             : 
+    2323             :   // remember this time, later check for timeout
+    2324          80 :   midair_activation_started_ = ros::Time::now();
+    2325             : 
+    2326             :   // start the timer which should check if the offboard is on, activate proper controller and tracker or timeout
+    2327          80 :   timer_midair_activation_.start();
+    2328             : 
+    2329          80 :   std::stringstream ss;
+    2330          80 :   ss << "midair activation initiated, starting the timer";
+    2331          80 :   ROS_INFO_STREAM_THROTTLE(1.0, "[UavManager]: " << ss.str());
+    2332             : 
+    2333          80 :   return std::tuple(true, ss.str());
+    2334             : }
+    2335             : 
+    2336             : //}
+    2337             : 
+    2338             : // | ----------------- service client wrappers ---------------- |
+    2339             : 
+    2340             : /* setOdometryCallbacksSrv() //{ */
+    2341             : 
+    2342         129 : void UavManager::setOdometryCallbacksSrv(const bool& input) {
+    2343             : 
+    2344         129 :   ROS_INFO("[UavManager]: switching odometry callbacks to %s", input ? "ON" : "OFF");
+    2345             : 
+    2346         258 :   std_srvs::SetBool srv;
+    2347             : 
+    2348         129 :   srv.request.data = input;
+    2349             : 
+    2350         129 :   bool res = sch_odometry_callbacks_.call(srv);
+    2351             : 
+    2352         129 :   if (res) {
+    2353             : 
+    2354         129 :     if (!srv.response.success) {
+    2355           0 :       ROS_WARN("[UavManager]: service call for toggle odometry callbacks returned: %s.", srv.response.message.c_str());
+    2356             :     }
+    2357             : 
+    2358             :   } else {
+    2359           0 :     ROS_ERROR("[UavManager]: service call for toggle odometry callbacks failed!");
+    2360             :   }
+    2361         129 : }
+    2362             : 
+    2363             : //}
+    2364             : 
+    2365             : /* setControlCallbacksSrv() //{ */
+    2366             : 
+    2367         102 : void UavManager::setControlCallbacksSrv(const bool& input) {
+    2368             : 
+    2369         102 :   ROS_INFO("[UavManager]: switching control callbacks to %s", input ? "ON" : "OFF");
+    2370             : 
+    2371         204 :   std_srvs::SetBool srv;
+    2372             : 
+    2373         102 :   srv.request.data = input;
+    2374             : 
+    2375         102 :   bool res = sch_control_callbacks_.call(srv);
+    2376             : 
+    2377         102 :   if (res) {
+    2378             : 
+    2379         102 :     if (!srv.response.success) {
+    2380           0 :       ROS_WARN("[UavManager]: service call for setting control callbacks returned: %s.", srv.response.message.c_str());
+    2381             :     }
+    2382             : 
+    2383             :   } else {
+    2384           0 :     ROS_ERROR("[UavManager]: service call for setting control callbacks failed!");
+    2385             :   }
+    2386         102 : }
+    2387             : 
+    2388             : //}
+    2389             : 
+    2390             : /* ungripSrv() //{ */
+    2391             : 
+    2392          23 : void UavManager::ungripSrv(void) {
+    2393             : 
+    2394          23 :   ROS_DEBUG_THROTTLE(1.0, "[UavManager]: ungripping payload");
+    2395             : 
+    2396          46 :   std_srvs::Trigger srv;
+    2397             : 
+    2398          23 :   bool res = sch_ungrip_.call(srv);
+    2399             : 
+    2400          23 :   if (res) {
+    2401             : 
+    2402           0 :     if (!srv.response.success) {
+    2403           0 :       ROS_DEBUG_THROTTLE(1.0, "[UavManager]: service call for ungripping payload returned: %s.", srv.response.message.c_str());
+    2404             :     }
+    2405             : 
+    2406             :   } else {
+    2407          23 :     ROS_DEBUG_THROTTLE(1.0, "[UavManager]: service call for ungripping payload failed!");
+    2408             :   }
+    2409          23 : }
+    2410             : 
+    2411             : //}
+    2412             : 
+    2413             : /* toggleControlOutput() //{ */
+    2414             : 
+    2415          85 : bool UavManager::toggleControlOutput(const bool& input) {
+    2416             : 
+    2417          85 :   ROS_DEBUG_THROTTLE(1.0, "[UavManager]: toggling control output %s", input ? "ON" : "OFF");
+    2418             : 
+    2419         170 :   std_srvs::SetBool srv;
+    2420             : 
+    2421          85 :   srv.request.data = input;
+    2422             : 
+    2423          85 :   bool res = sch_toggle_control_output_.call(srv);
+    2424             : 
+    2425          85 :   if (res) {
+    2426             : 
+    2427          84 :     if (!srv.response.success) {
+    2428           0 :       ROS_DEBUG_THROTTLE(1.0, "[UavManager]: service call for control output returned: %s.", srv.response.message.c_str());
+    2429           0 :       return false;
+    2430             :     } else {
+    2431          84 :       return true;
+    2432             :     }
+    2433             : 
+    2434             :   } else {
+    2435           1 :     ROS_DEBUG_THROTTLE(1.0, "[UavManager]: service call for control output failed!");
+    2436           1 :     return false;
+    2437             :   }
+    2438             : }
+    2439             : 
+    2440             : //}
+    2441             : 
+    2442             : /* offboardSrv() //{ */
+    2443             : 
+    2444          81 : bool UavManager::offboardSrv(const bool in) {
+    2445             : 
+    2446          81 :   ROS_DEBUG_THROTTLE(1.0, "[UavManager]: setting offboard to %d", in);
+    2447             : 
+    2448         162 :   std_srvs::Trigger srv;
+    2449             : 
+    2450          81 :   bool res = sch_offboard_.call(srv);
+    2451             : 
+    2452          81 :   if (!res) {
+    2453             : 
+    2454           1 :     ROS_DEBUG_THROTTLE(1.0, "[UavManager]: service call for offboard failed!");
+    2455           1 :     return false;
+    2456             : 
+    2457             :   } else {
+    2458             : 
+    2459          80 :     if (!srv.response.success) {
+    2460           0 :       ROS_DEBUG_THROTTLE(1.0, "[UavManager]: service call for offboard failed, returned: %s", srv.response.message.c_str());
+    2461           0 :       return false;
+    2462             :     } else {
+    2463          80 :       return true;
+    2464             :     }
+    2465             :   }
+    2466             : }
+    2467             : 
+    2468             : //}
+    2469             : 
+    2470             : /* disarmSrv() //{ */
+    2471             : 
+    2472           7 : void UavManager::disarmSrv(void) {
+    2473             : 
+    2474          14 :   std_srvs::SetBool srv;
+    2475             : 
+    2476           7 :   srv.request.data = false;
+    2477             : 
+    2478           7 :   bool res = sch_arm_.call(srv);
+    2479             : 
+    2480           7 :   if (res) {
+    2481             : 
+    2482           7 :     if (!srv.response.success) {
+    2483           0 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: service call disarming returned: %s.", srv.response.message.c_str());
+    2484             :     }
+    2485             : 
+    2486             :   } else {
+    2487           0 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: service call for disarming failed!");
+    2488             :   }
+    2489           7 : }
+    2490             : 
+    2491             : //}
+    2492             : 
+    2493             : /* switchControllerSrv() //{ */
+    2494             : 
+    2495         213 : bool UavManager::switchControllerSrv(const std::string& controller) {
+    2496             : 
+    2497         213 :   ROS_INFO_STREAM("[UavManager]: activating controller '" << controller << "'");
+    2498             : 
+    2499         426 :   mrs_msgs::String srv;
+    2500         213 :   srv.request.value = controller;
+    2501             : 
+    2502         213 :   bool res = sch_switch_controller_.call(srv);
+    2503             : 
+    2504         213 :   if (res) {
+    2505             : 
+    2506         213 :     if (!srv.response.success) {
+    2507           2 :       ROS_WARN("[UavManager]: service call for switching controller returned: '%s'", srv.response.message.c_str());
+    2508             :     }
+    2509             : 
+    2510         213 :     return srv.response.success;
+    2511             : 
+    2512             :   } else {
+    2513             : 
+    2514           0 :     ROS_ERROR("[UavManager]: service call for switching controller failed!");
+    2515             : 
+    2516           0 :     return false;
+    2517             :   }
+    2518             : }
+    2519             : 
+    2520             : //}
+    2521             : 
+    2522             : /* switchTrackerSrv() //{ */
+    2523             : 
+    2524         214 : bool UavManager::switchTrackerSrv(const std::string& tracker) {
+    2525             : 
+    2526         214 :   ROS_INFO_STREAM("[UavManager]: activating tracker '" << tracker << "'");
+    2527             : 
+    2528             : 
+    2529         428 :   mrs_msgs::String srv;
+    2530         214 :   srv.request.value = tracker;
+    2531             : 
+    2532         214 :   bool res = sch_switch_tracker_.call(srv);
+    2533             : 
+    2534         214 :   if (res) {
+    2535             : 
+    2536         214 :     if (!srv.response.success) {
+    2537           2 :       ROS_WARN("[UavManager]: service call for switching tracker returned: '%s'", srv.response.message.c_str());
+    2538             :     }
+    2539             : 
+    2540         214 :     return srv.response.success;
+    2541             : 
+    2542             :   } else {
+    2543             : 
+    2544           0 :     ROS_ERROR("[UavManager]: service call for switching tracker failed!");
+    2545             : 
+    2546           0 :     return false;
+    2547             :   }
+    2548             : }
+    2549             : 
+    2550             : //}
+    2551             : 
+    2552             : /* landSrv() //{ */
+    2553             : 
+    2554           5 : bool UavManager::landSrv(void) {
+    2555             : 
+    2556           5 :   ROS_INFO("[UavManager]: calling for landing");
+    2557             : 
+    2558          10 :   std_srvs::Trigger srv;
+    2559             : 
+    2560           5 :   bool res = sch_land_.call(srv);
+    2561             : 
+    2562           5 :   if (res) {
+    2563             : 
+    2564           5 :     if (!srv.response.success) {
+    2565           0 :       ROS_WARN("[UavManager]: service call for landing returned: '%s'", srv.response.message.c_str());
+    2566             :     }
+    2567             : 
+    2568           5 :     return srv.response.success;
+    2569             : 
+    2570             :   } else {
+    2571             : 
+    2572           0 :     ROS_ERROR("[UavManager]: service call for landing failed!");
+    2573             : 
+    2574           0 :     return false;
+    2575             :   }
+    2576             : }
+    2577             : 
+    2578             : //}
+    2579             : 
+    2580             : /* elandSrv() //{ */
+    2581             : 
+    2582           1 : bool UavManager::elandSrv(void) {
+    2583             : 
+    2584           1 :   ROS_INFO("[UavManager]: calling for eland");
+    2585             : 
+    2586           2 :   std_srvs::Trigger srv;
+    2587             : 
+    2588           1 :   bool res = sch_eland_.call(srv);
+    2589             : 
+    2590           1 :   if (res) {
+    2591             : 
+    2592           1 :     if (!srv.response.success) {
+    2593           0 :       ROS_WARN("[UavManager]: service call for eland returned: '%s'", srv.response.message.c_str());
+    2594             :     }
+    2595             : 
+    2596           1 :     return srv.response.success;
+    2597             : 
+    2598             :   } else {
+    2599             : 
+    2600           0 :     ROS_ERROR("[UavManager]: service call for eland failed!");
+    2601             : 
+    2602           0 :     return false;
+    2603             :   }
+    2604             : }
+    2605             : 
+    2606             : //}
+    2607             : 
+    2608             : /* ehoverSrv() //{ */
+    2609             : 
+    2610           2 : bool UavManager::ehoverSrv(void) {
+    2611             : 
+    2612           2 :   ROS_INFO("[UavManager]: calling for ehover");
+    2613             : 
+    2614           4 :   std_srvs::Trigger srv;
+    2615             : 
+    2616           2 :   bool res = sch_ehover_.call(srv);
+    2617             : 
+    2618           2 :   if (res) {
+    2619             : 
+    2620           2 :     if (!srv.response.success) {
+    2621           0 :       ROS_WARN("[UavManager]: service call for ehover returned: '%s'", srv.response.message.c_str());
+    2622             :     }
+    2623             : 
+    2624           2 :     return srv.response.success;
+    2625             : 
+    2626             :   } else {
+    2627             : 
+    2628           0 :     ROS_ERROR("[UavManager]: service call for ehover failed!");
+    2629             : 
+    2630           0 :     return false;
+    2631             :   }
+    2632             : }
+    2633             : 
+    2634             : //}
+    2635             : 
+    2636             : /* takeoffSrv() //{ */
+    2637             : 
+    2638          21 : bool UavManager::takeoffSrv(void) {
+    2639             : 
+    2640          21 :   ROS_INFO("[UavManager]: calling for takeoff to height '%.2f m'", _takeoff_height_);
+    2641             : 
+    2642          42 :   mrs_msgs::Vec1 srv;
+    2643             : 
+    2644          21 :   srv.request.goal = _takeoff_height_;
+    2645             : 
+    2646          21 :   bool res = sch_takeoff_.call(srv);
+    2647             : 
+    2648          21 :   if (res) {
+    2649             : 
+    2650          21 :     if (!srv.response.success) {
+    2651           0 :       ROS_WARN("[UavManager]: service call for takeoff returned: '%s'", srv.response.message.c_str());
+    2652             :     }
+    2653             : 
+    2654          21 :     return srv.response.success;
+    2655             : 
+    2656             :   } else {
+    2657             : 
+    2658           0 :     ROS_ERROR("[UavManager]: service call for takeoff failed!");
+    2659             : 
+    2660           0 :     return false;
+    2661             :   }
+    2662             : }
+    2663             : 
+    2664             : //}
+    2665             : 
+    2666             : /* emergencyReferenceSrv() //{ */
+    2667             : 
+    2668         101 : bool UavManager::emergencyReferenceSrv(const mrs_msgs::ReferenceStamped& goal) {
+    2669             : 
+    2670         101 :   ROS_INFO_THROTTLE(1.0, "[UavManager]: calling for emergency reference");
+    2671             : 
+    2672         202 :   mrs_msgs::ReferenceStampedSrv srv;
+    2673             : 
+    2674         101 :   srv.request.header    = goal.header;
+    2675         101 :   srv.request.reference = goal.reference;
+    2676             : 
+    2677         101 :   bool res = sch_emergency_reference_.call(srv);
+    2678             : 
+    2679         101 :   if (res) {
+    2680             : 
+    2681         101 :     if (!srv.response.success) {
+    2682           0 :       ROS_WARN_THROTTLE(1.0, "[UavManager]: service call for emergency reference returned: '%s'", srv.response.message.c_str());
+    2683             :     }
+    2684             : 
+    2685         101 :     return srv.response.success;
+    2686             : 
+    2687             :   } else {
+    2688             : 
+    2689           0 :     ROS_ERROR_THROTTLE(1.0, "[UavManager]: service call for emergency reference failed!");
+    2690             : 
+    2691           0 :     return false;
+    2692             :   }
+    2693             : }
+    2694             : 
+    2695             : //}
+    2696             : 
+    2697             : }  // namespace uav_manager
+    2698             : 
+    2699             : }  // namespace mrs_uav_managers
+    2700             : 
+    2701             : #include <pluginlib/class_list_macros.h>
+    2702         109 : 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..be368f81e6 --- /dev/null +++ b/mrs_uav_managers/src/uav_manager.cpp.gcov.overview.html @@ -0,0 +1,696 @@ + + + + + + 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 + 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..6f558db57aa187472808ec2fff9310197eca290b GIT binary patch literal 7404 zcmVk#Xr)WX9peO{2Daxysw~+>S0>ypJK(U@d@!~Ed zDAqjTKq*D6?rxw?Ji@9s4xEbOHgXk3h4zSnGC9iIsHmE(9RZ3dih)90bBVxY3>acFj<16tO-L(g1&D>)T=74w=w)eYJ^R6RJ2 zTw7p8ai!M=u1pIk+H-vo>z@v2;|9AT(nVESiQxPSwsi|@i z2D2ns`Q*PaQdI<@7zxLX88Nab<2I|SsT%nvaKfUv>NZ}tt-@5|8GT*qS_q2y>^6)^ ze;wC)igXdVi&z`I30Q6PxRM#%6mT8k81_FM!S?}6?%zOh2lDp8JY zzXuI+`H2Fw0|$<^6ex;gImJ=Hq6Ix%H-UHy{ZEH$te9xqm;sqlCt8VOHb+l1TvDHvXiD9!Q z5O4xCgVjedkVkd_ld>bS$DKHYq!=hJS1{f2K(2JH_}$93X~99{15`NRCIy@8aR0Fc zQdJkSCTf-!1=mH@R6uMOt|5?ScNg)DL8b6{1GW(|Oam`Dkdb4#f?}B$G2^#~8oj-- zjawR7MZKsVtUZ8@T^$+DTqth2mjjV*=YV}p4HHrb2@Tf}!W(mvxoDVbvsRmI-MvDH zPR-a6vsP^!ii<5zJdj~~A;rP-(Xx?c7jLK&R4odcthKd*84h^K7L??WS-%Thu7TRi zF01B%l`Gz8qe=f=E~>rC%AApzK(*bI0IAV{P*iY;QdDSy!XV)Wv zVt$OSAwZSS*wsn9gMQlU%+oZt_ z;80y9@XcDzY?6$9#_x)8L++d*>pdjYgaUDW6s;f$pn}VqVkJl1gm0*WsCk!!Q`-X~ z%Q7vY%=3*bms>#57zbpC6cnYvo#gl`X4$(?j;>4_xxW4O$AXo)1j;GG!U6jzE=~v} zFgAQ9YcJGnMmeslhLR$Iv!_TXXkB#^=qXYfpS3YEgY;ydzzOwOOK%rWl@>>e2O8Ig z7BU~#w1w>K!a@-?Q=Ez~<{YS_nDncm;Xo1jF_VCF)w(w31v3LE5F!^2IF%@68`JMk zGp#__cUKD39 zob~T&Vo+0zOQ|7+A42kQgX;;?iS{UDfFg?Ae68Yo@DUfE06hR1j3EFxWSrr_H_`0pl2p2(1C6GI!!FRhG$@58&%90jKW>p zxDU{Z>!4_D;IvyC6=xfncz#zw{2oL;v&Pt`RW7|;zx^jqYI7F~fRn1P8x+&BS#I|J zZ`Ht6X;GXL@MGH;!Itw(FexTn1KKK&G)1q6YxGZJBwKAnc&=yA#(Yshh$xdIv;-JX zE=R<!v;?Lzybjc2U+ zjj?7@+m>Vi{O`4aVv3%w1USp8mbUSxU`AN=q7jb_I7=ytt8P=2Y7b@r^7NQ413)*; z7tKZNQjN38i-L``jx^^;J$*EhDYmZ zX8>j?Fo1<-Pi{!j5U;wkjsEVf?S?cP^i%dv zka2e#^EJvQIj;1{uAUb?urUasK8k@0J25G$t_!FkUz}~GIQK0{Z`o27uX~znibNx4 z)o_gp*3ceuuX3?ihzA8XSp<-?cDRgh9l=8Q{!R8cI$SIKbCSwvGx(JMwc z(MfB?3!NtOyb;~Z+_1)DK#>iQE3T_q)J6eHS#>rvxT?i%lv&7~ZUx0MH)^g+<1o8F z^Qyo}n~q`Z$>e!&TN5z*z3K!4017Ggwb3MJ5jXZJ6CoJ64r8C}0P6C=MJ^jg+O`XT zjmdoTGZ^#r&zdY28E`;>_U^lgU*KCUMRtk>q2+#}%LHuuTbE0*8b*1ZVH;DgWZSM7 zGZ-+-YgUK{NW7}Ci(D8Z;(HS=qxr&RGaAjhmvo^lR72jcY{L z#X$8?NGowj4FmR2WDg?V_?0@W+Uo(qZUPDyXD_9A{t)H)^MFc`{UnZ^ACT<(TRgC> zXTLz%pizt&%)fT}5qrXIEZ(YUYRHwyB&!RNxEiLCN(Gctba6?(D>Vjy=tFPr{GV5n zwybM3JyV*Bx|ej>ia9Tjz>;TTO`2N&m0Po%VxoxwWfU*c*vB2z{isG^XVuQT54+JL zw%8vprBcNDQLc+-J&`8Xz+YMp_;_S=aUjOb{{TJuR7k@vBr)GNOW+bu2VDxilqfzROPYTfFI*a~rSm~f zcAeWUj2EEZcDedCkJtDrN-y_gS~~NBMP4sL=_tihs2LoU z@w-|zs|yb7q=*WkdnjU{xgijs(?O3mm#e7dinAGGlK2_u3K_zugIOedb{BN&+5m!vqblOigF?x8@AuMI=N zYnESGZ=~1;?mA{DiT8J5h)kAr`zf+O>6kI!jyIk&sTe7g=sG1S7UEK#cgXoVvmZF% z_hvuf6|0lJ;P$>nECt_eT8cvmnzI16>}bTqoS+qMiOUt2v=mQZeEn{UNY8dKDdG}3 zrnRT~_P(B$w9X?>EJ`U};6?Qf3CZ&KZ?~Y3#P&L1MwTgk7j8GxTa`cgc5?tWbxjrK zzWTSPeU7nzrUpEP6W{W4@5PMa2%8V6hAR39-2lJ#0~RNj z2meu?NG`8BDo$kLJ$d$UFD~1cyVL>cCH29a&WyY{&z?;MPF44aXGT4nDNuYT*Ec?u zDl+2+M{$!w)gWD_vIA^-Zx-W~iLuvIdQKz#FXc z4rTjVn=>2)M6QseIm2Xx#C?AJq?|bE`g)IxODE80#Erir^HDg4~upUpibnbUF!Da>)^t(aJ7y2e z*&|Ox?9XeaOyDPk?AD&i4s`V9?TPYX`YEO)sj19h>7vMldo`2hxopm?#i5tYfq@BR zNo_k;{qBW0xeh-U;^#sfr}(yokCdWtF9qIbf3A>FHG}>r?M0fdkd_6x)lEq8Tp>lS z^m+CLy>o@sNfDVf%_tsqg#^r=&`8~nyh7S^);=4AoBR4)AsJBwS0>s#=8NPLBO6%v#p z2VBV2PXWxr3NfEo7gexdArWm{!eRWN!C4zq=PhMf+i2OqSFvWgTF+1mhP@vgU+cyV z+UBKH*@GFytOZqmk6MoHHXcom3{RaD;Z8-!D|ReQkYaI|Wn<3>ZnOQ?)$rjxsjplO z^RXEfP+m3C^y%iEG&dY49Wys*9s>0`WwS6TlvCAVq>!06zse|4Yy+hf_x75TvejHm z#k0~O_eM%!E1c1@18EoU35Sc~>w(m(+2v@qU?y90)q_P(F&?$!kKA=`KrnBy^ezE> zf3iUC1sHwR#Es2x9ipQs$(3FyMi!Whsxu&WqxB0YJ*M@xLq3^?GeUa1hO>DjuGpv< zRJNVf44Eru99Z+50S};N_!z0$c(g#J&3+T&UPoy`Py1NzbsR?VA(mRhwafs)Y@26t z5r>uiF=k&k_QX{?cyvD(%KsZB25tFs~Uo*0P7!{AG7P)FsaI5O}o3+5USU zL0B&j`EZM~qU2Q{qp#sR+2N)sDeS(>yoG(i!%*5*MD4&gjQB^^bZ!%T%U4Wkbng$$EcYY*MCcns!$W++gGVg{(AMw zl7bspew~;suZwxJcqVyepN<5kD_Y$ngR0sb4>=pTA9xJ4CreTNR`SP(Gpf zM=eF^N#XN9>Hz>Bpm+!Jlp`rhRF0S^PdT!~TXXsfH2~|mCAb(k1-?%A)aH;*;9yE4 z-W*lF!~&moc|3@RPIQlM{J0{OPczuSWZtk0_di>S-`T*Miw=yzBzS!Us4x=hV zV0OJxI#09y!Q;Z67ne50qr20EzaL+-X}8CnF0ukBGOy`+Ao7|n`eu`4uqI&3*5|-B zGi_VH({-&$p%;HA64uvZ#jR4RNuZSCCFCE+Kr=NYl<~(osXm@l5%cU5HzZKX%zEs6 zxTaSaMv5nwd_7*rbVfbC;F~34$6Z2qqMhMdl3CQ3uFM8<-D=qBm(SqNqidc#l6uSM=M=H*KnPQO;>EV z;y_&{AU<4?2cAXwRR4c)O|$?0Ik-esA7F5~|NZzH;*}+1$QT(taD<5?LwX-5Oe9Wm zMdH@Z*ZFJ}?Lp1V;BR3W{q%)TV4?Nt%9em7k0iC_2uxHd6i_rqD(wzi+yL!t&v*r| zHn}eKU&5;4iru0Ma{&#(!@wBd;khVL8zIHa5){`apjn@;9pD_NsUdan=A^Mye2X?3pZCLeDjK!yr<%6v&X(4%cMFgi2vOUxUTbmKSaIyo8aB7R1P~ z3m2r+Ulb^b7H9*$vJkSUIHee)`SvbDdYk=Pis#d>+iH7ehe@8>{ViW_T#8S#VqAYr zs@oKI&WeVdscz2bQouXhsEC12Cbh_V`$}vdkC+mJXBHq5e*o?i*Zb`FG$nq(lo%u( z<_Da}(Qy4GTWoTH_oT!`ea+IiA~S!`QdDvz;(NFp5ea;m91pbRy9P*^$3e9FT#l9z zP*e36I>S4j8uG(7|8z`em^k=xI>T<~gSu|(4oveiCE8c^_sdMPw493y7HB^fZrc|X z^+B6+QE@IRN;!VyqT&gR#vip5=c0lI_Q{dk{JfWPT+?-HS)FoRl8Y%H*A4L;UXI6D z{u&s=>pn(3XteAVpq3}dDXCflChdyE zu^0aKh^HBoib+iqc`^45Go(=j8()@erAcK#7OI>yW2z;aT9i2EUQP(W^@9XRDy*KF zYQ82~P<0_tV3Vrp-sz-h)7R$D9BGefJ$!tYH2L*0Eqp%GA0-nW(-Iy0+U1zmSMK=i zcO79pfDfDggl9mVJ3cGfbEhH$+V|n@+^OJz;*!hp+^MMK_>nsmXZq8*Q^5oK zl)yh5VQ=N6DJilt;;YByu%tD>%9LK00MXVevT;o+jLCb{-()D_+pFCGe?6Lt0B)}_j| zKclWIS>gK%blg?D(zIXXzCQ-wI!N-qf|Db*FLuh2)yi6q{@PZO>r;;H7-{$?+?4@Z z{zv7w_ihS(oc!B%0mO>i_rAg!NCio5&#z}p!>q4^Kt{5QUqNL?l<6qF7PME3c55x z=N$)@_`rv5?;~777Vk+0P4>gBb2o+EPN@$Kkic26Pgzss?tk24S1MOl?Df)HuaV-T zToK4~^=FM#ZlkW(+-P;Z-zBd^&OQ*Dx0|iMZ3B19;R2&K;i_%8(r5ge%j$( zW5(pMh`Y_0dBj%-&oN_F@|Ok}V9!ef6LvAKT6=E71Ru@d5@xX6gc&RmxPFoVmjpLr z<{F>cAlSJH{cGj%^lW~`jGmsE>ovVAHC`#jgl~w@+Iz!8!<8?vwvlHgF|+e)2o__e z&Q>y|NG3|5Rbz=#y;U2(3m|+fVqp@7>ozWAhM%^LQ$~&g$|!!!l@5Yn>N&BaqiN20WGJ1B4!GTP8Urn zQcf;D6c8u3yK0-onaF|ESzKEO@_v^&ka?aIa|a8SCxxB{*dtkKk9VcYt#gC-uR(B+ z9Ek_n8iE@`7TYGlXVRtO9>fJN!6=edEh$5SQ=cSnQHU%d>u@rdJ>4 z2HL{`V|cFyP)BYC@m03&3~2EsYscq#hR+hA)n~Ume00nfod?q^x+9hLqSaPu1vx1<`|} z$@W@Y>)pLnXtM{5D`OvJX76rk>>sDG4==J_K@h`#Scg%WCuN{LzG}gH>0v);jPHz( zxCzitNMLAfVg?K@v#SV7V=o5M%w!nmti8UCnkjAx`zmPYv*6^9OmWfwAHWP&D{d~D z@*a?kYauhSwjnG(!o57{U&H`d+*u&fom}zuy4ZT_#f*0P^Ig znCr>_EYOYYbqmc6Kt5az+-vLM`U<5$6(<$99i#{QCg86PFg?7gMbGbuBItk z$b4P7>lVW`Oxt13{uA@{rl;UPXuihg#K%vHkv8nW%Th2F2LNv&-qU7A8t)MoGuz{@ zdP^yTO=hG<>PUH{c1}fQq*g&A_j1Xk$QUVSsHSLXKFX)qe!8GC>zmuyoQ9pJrWgv+ zs_kz$HMEcJ!!hjR7v``ZV?A>qcgBL^hqyD3G-{t9_Tb!^&j>%D7R7z@E5i@}VLcP4 zC-spNbVt*#FvG5<_x7|Dr{>OCuoO%Zsg{}V2`kvmva?oMiCwwF!87GLjzO!Y3j3P;RxcA9IB|K-4S-upX#9) z?{sfg9S^)3ylanSD{i6qA!e`K-cK9+r`h8&3!bOh;z2 literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.func-sort-c.html new file mode 100644 index 0000000000..73e84da978 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.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/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-12-17 22:35:29Functions: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()80
mrs_uav_state_estimators::garmin_agl::GarminAgl::~GarminAgl()80
mrs_uav_state_estimators::garmin_agl::GarminAgl::~GarminAgl().280
+
+
+ + + +
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..d015d074cd --- /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-12-17 22:35:29Functions: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()80
mrs_uav_state_estimators::garmin_agl::GarminAgl::~GarminAgl()80
mrs_uav_state_estimators::garmin_agl::GarminAgl::~GarminAgl().280
+
+
+ + + +
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..d60478a12a --- /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-12-17 22:35:29Functions: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          80 :   GarminAgl() : AglEstimator(garmin_agl::name, garmin_agl::frame_id, garmin_agl::package_name), is_core_plugin_(is_core_plugin) {
+      57          80 :   }
+      58             : 
+      59         160 :   ~GarminAgl(void) {
+      60         160 :   }
+      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-12-17 22:35:29Functions: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..09640d387f --- /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-12-17 22:35:29Functions: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..83d4342ba8 --- /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-12-17 22:35:29Functions: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..e18c5f0daa --- /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-12-17 22:35:29Functions: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..f8f4fc1ed9 --- /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-12-17 22:35:29Functions: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..aa9cb17992 --- /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-12-17 22:35:29Functions: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..a364cf48e8 --- /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-12-17 22:35:29Functions: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)196
mrs_uav_state_estimators::AltGeneric::~AltGeneric()196
mrs_uav_state_estimators::AltGeneric::~AltGeneric().2196
+
+
+ + + +
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..7868868ad2 --- /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-12-17 22:35:29Functions: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)196
mrs_uav_state_estimators::AltGeneric::~AltGeneric()196
mrs_uav_state_estimators::AltGeneric::~AltGeneric().2196
+
+
+ + + +
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..9480d73028 --- /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-12-17 22:35:29Functions: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         196 :   AltGeneric(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name, const bool is_core_plugin)
+     115         196 :       : AltitudeEstimator<alt_generic::n_states>(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), is_core_plugin_(is_core_plugin) {
+     116         196 :   }
+     117             : 
+     118         392 :   ~AltGeneric(void) {
+     119         392 :   }
+     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-12-17 22:35:29Functions: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&)196
mrs_uav_state_estimators::AltitudeEstimator<3>::~AltitudeEstimator().2196
+
+
+ + + +
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..f6370fc87d --- /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-12-17 22:35:29Functions: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&)196
mrs_uav_state_estimators::AltitudeEstimator<3>::~AltitudeEstimator()0
mrs_uav_state_estimators::AltitudeEstimator<3>::~AltitudeEstimator().2196
+
+
+ + + +
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..693106458a --- /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-12-17 22:35:29Functions: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         196 :   AltitudeEstimator(const std::string& name, const std::string& frame_id) : PartialEstimator<n_states, 1>(altitude::type, name, frame_id) {
+      31         196 :   }
+      32             : 
+      33         196 :   virtual ~AltitudeEstimator(void) {
+      34         196 :   }
+      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..ab92c4733a --- /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-12-17 22:35:29Functions: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..7971872211 --- /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-12-17 22:35:29Functions: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..032f930088 --- /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-12-17 22:35:29Functions: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..4572ad04e8 --- /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-12-17 22:35:29Functions: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..4ec211166d --- /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-12-17 22:35:29Functions: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..dcca03c482 --- /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-12-17 22:35:29Functions: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..54dc8dbe39 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.func-sort-c.html @@ -0,0 +1,428 @@ + + + + + + + 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:382108935.1 %
Date:2024-12-17 22:35:29Functions:488755.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::Correction<1>::getInFrame(geometry_msgs::Point_<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>::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>::callbackMagField(boost::shared_ptr<sensor_msgs::MagneticField_<std::allocator<void> > const>)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>::callbackNavSatFix(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::callbackMagHeading(boost::shared_ptr<mrs_msgs::Float64Stamped_<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>::transformVecToFrame(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>::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>::getCorrectionFromMagField(boost::shared_ptr<sensor_msgs::MagneticField_<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>::getCorrectionFromNavSatFix(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromMagHeading(boost::shared_ptr<mrs_msgs::Float64Stamped_<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>::getInFrame(geometry_msgs::Point_<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<2>::callbackImu(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getAvgInitZ(double)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>::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>::callbackMagField(boost::shared_ptr<sensor_msgs::MagneticField_<std::allocator<void> > const>)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>::callbackNavSatFix(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::callbackMagHeading(boost::shared_ptr<mrs_msgs::Float64Stamped_<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>::getCorrectionFromMagField(boost::shared_ptr<sensor_msgs::MagneticField_<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>::getCorrectionFromNavSatFix(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromMagHeading(boost::shared_ptr<mrs_msgs::Float64Stamped_<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>::getAvgInitZ(double)22
mrs_uav_state_estimators::Correction<2>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)116
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)>)120
mrs_uav_state_estimators::Correction<2>::getName[abi:cxx11]() const240
mrs_uav_state_estimators::Correction<2>::getPrintName[abi:cxx11]() const244
mrs_uav_state_estimators::Correction<1>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)254
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)>)282
mrs_uav_state_estimators::Correction<2>::getNamespacedName[abi:cxx11]() const518
mrs_uav_state_estimators::Correction<1>::getName[abi:cxx11]() const564
mrs_uav_state_estimators::Correction<1>::getPrintName[abi:cxx11]() const619
mrs_uav_state_estimators::Correction<1>::getNamespacedName[abi:cxx11]() const1105
mrs_uav_state_estimators::Correction<1>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)1799
mrs_uav_state_estimators::Correction<1>::getCorrectionFromRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)1831
mrs_uav_state_estimators::Correction<1>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const1831
mrs_uav_state_estimators::Correction<2>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)3800
mrs_uav_state_estimators::Correction<2>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const3875
mrs_uav_state_estimators::Correction<2>::getCorrectionFromRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)3878
mrs_uav_state_estimators::Correction<2>::getRawCorrection()7936
mrs_uav_state_estimators::Correction<2>::getProcessedCorrection()7936
mrs_uav_state_estimators::Correction<1>::getRawCorrection()8950
mrs_uav_state_estimators::Correction<1>::getProcessedCorrection()8952
mrs_uav_state_estimators::Correction<2>::callbackVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)11020
mrs_uav_state_estimators::Correction<2>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)11049
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> >)11054
mrs_uav_state_estimators::Correction<2>::transformVecToFrame(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> >)11054
mrs_uav_state_estimators::Correction<2>::callbackPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)218493
mrs_uav_state_estimators::Correction<2>::getCorrectionFromPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)222580
mrs_uav_state_estimators::Correction<2>::isMsgComing()225376
mrs_uav_state_estimators::Correction<2>::isHealthy()225377
mrs_uav_state_estimators::Correction<1>::callbackVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)228707
mrs_uav_state_estimators::Correction<1>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)228800
mrs_uav_state_estimators::Correction<1>::getZVelUntilted(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&)228837
mrs_uav_state_estimators::Correction<2>::setR(double)229450
mrs_uav_state_estimators::Correction<2>::getStateId() const229785
mrs_uav_state_estimators::Correction<2>::applyCorrection(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, ros::Time const&)233260
mrs_uav_state_estimators::Correction<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)237496
mrs_uav_state_estimators::Correction<1>::callbackRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)265563
mrs_uav_state_estimators::Correction<1>::getCorrectionFromRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)268897
mrs_uav_state_estimators::Correction<2>::publishCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)466889
mrs_uav_state_estimators::Correction<1>::isMsgComing()484114
mrs_uav_state_estimators::Correction<1>::isHealthy()484147
mrs_uav_state_estimators::Correction<1>::setR(double)492133
mrs_uav_state_estimators::Correction<1>::getStateId() const492162
mrs_uav_state_estimators::Correction<1>::applyCorrection(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&)496218
mrs_uav_state_estimators::Correction<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)500483
mrs_uav_state_estimators::Correction<1>::publishCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)992618
mrs_uav_state_estimators::Correction<2>::getR()1162820
mrs_uav_state_estimators::Correction<1>::getR()1484354
+
+
+ + + +
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..f891914fd4 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.func.html @@ -0,0 +1,428 @@ + + + + + + + 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:382108935.1 %
Date:2024-12-17 22:35:29Functions:488755.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::Correction<1>::getInFrame(geometry_msgs::Point_<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>::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>)1799
mrs_uav_state_estimators::Correction<1>::getAvgInitZ(double)22
mrs_uav_state_estimators::Correction<1>::isMsgComing()484114
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>)265563
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>)228707
mrs_uav_state_estimators::Correction<1>::applyCorrection(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&)496218
mrs_uav_state_estimators::Correction<1>::getZVelUntilted(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&)228837
mrs_uav_state_estimators::Correction<1>::callbackMagField(boost::shared_ptr<sensor_msgs::MagneticField_<std::allocator<void> > const>)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>::getRawCorrection()8950
mrs_uav_state_estimators::Correction<1>::callbackNavSatFix(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::publishCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)992618
mrs_uav_state_estimators::Correction<1>::callbackMagHeading(boost::shared_ptr<mrs_msgs::Float64Stamped_<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>::transformVecToFrame(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>::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>)1831
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>)268897
mrs_uav_state_estimators::Correction<1>::getProcessedCorrection()8952
mrs_uav_state_estimators::Correction<1>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)254
mrs_uav_state_estimators::Correction<1>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)228800
mrs_uav_state_estimators::Correction<1>::getCorrectionFromMagField(boost::shared_ptr<sensor_msgs::MagneticField_<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>::getCorrectionFromNavSatFix(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromMagHeading(boost::shared_ptr<mrs_msgs::Float64Stamped_<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()1484354
mrs_uav_state_estimators::Correction<1>::setR(double)492133
mrs_uav_state_estimators::Correction<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)500483
mrs_uav_state_estimators::Correction<1>::isHealthy()484147
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)>)282
mrs_uav_state_estimators::Correction<2>::getInFrame(geometry_msgs::Point_<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<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>)3800
mrs_uav_state_estimators::Correction<2>::getAvgInitZ(double)0
mrs_uav_state_estimators::Correction<2>::isMsgComing()225376
mrs_uav_state_estimators::Correction<2>::callbackPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)218493
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> >)11054
mrs_uav_state_estimators::Correction<2>::callbackVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)11020
mrs_uav_state_estimators::Correction<2>::applyCorrection(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, ros::Time const&)233260
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>::callbackMagField(boost::shared_ptr<sensor_msgs::MagneticField_<std::allocator<void> > const>)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()7936
mrs_uav_state_estimators::Correction<2>::callbackNavSatFix(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::publishCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)466889
mrs_uav_state_estimators::Correction<2>::callbackMagHeading(boost::shared_ptr<mrs_msgs::Float64Stamped_<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>::transformVecToFrame(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> >)11054
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>)3878
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>)222580
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()7936
mrs_uav_state_estimators::Correction<2>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)116
mrs_uav_state_estimators::Correction<2>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)11049
mrs_uav_state_estimators::Correction<2>::getCorrectionFromMagField(boost::shared_ptr<sensor_msgs::MagneticField_<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>::getCorrectionFromNavSatFix(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromMagHeading(boost::shared_ptr<mrs_msgs::Float64Stamped_<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()1162820
mrs_uav_state_estimators::Correction<2>::setR(double)229450
mrs_uav_state_estimators::Correction<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)237496
mrs_uav_state_estimators::Correction<2>::isHealthy()225377
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)>)120
mrs_uav_state_estimators::Correction<1>::getStateId() const492162
mrs_uav_state_estimators::Correction<1>::getPrintName[abi:cxx11]() const619
mrs_uav_state_estimators::Correction<1>::getNamespacedName[abi:cxx11]() const1105
mrs_uav_state_estimators::Correction<1>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const1831
mrs_uav_state_estimators::Correction<1>::getName[abi:cxx11]() const564
mrs_uav_state_estimators::Correction<2>::getStateId() const229785
mrs_uav_state_estimators::Correction<2>::getPrintName[abi:cxx11]() const244
mrs_uav_state_estimators::Correction<2>::getNamespacedName[abi:cxx11]() const518
mrs_uav_state_estimators::Correction<2>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const3875
mrs_uav_state_estimators::Correction<2>::getName[abi:cxx11]() const240
+
+
+ + + +
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..08cbcf2120 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.html @@ -0,0 +1,2719 @@ + + + + + + + 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:382108935.1 %
Date:2024-12-17 22:35:29Functions:488755.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 <sensor_msgs/MagneticField.h>
+      23             : #include <sensor_msgs/NavSatFix.h>
+      24             : #include <nav_msgs/Odometry.h>
+      25             : 
+      26             : #include <std_srvs/SetBool.h>
+      27             : 
+      28             : #include <functional>
+      29             : 
+      30             : #include <mrs_uav_managers/estimation_manager/types.h>
+      31             : #include <mrs_uav_managers/estimation_manager/support.h>
+      32             : #include <mrs_uav_managers/estimation_manager/common_handlers.h>
+      33             : #include <mrs_uav_managers/estimation_manager/private_handlers.h>
+      34             : 
+      35             : #include <mrs_uav_state_estimators/processors/processor.h>
+      36             : #include <mrs_uav_state_estimators/processors/proc_median_filter.h>
+      37             : #include <mrs_uav_state_estimators/processors/proc_saturate.h>
+      38             : #include <mrs_uav_state_estimators/processors/proc_excessive_tilt.h>
+      39             : #include <mrs_uav_state_estimators/processors/proc_tf_to_world.h>
+      40             : #include <mrs_uav_state_estimators/processors/proc_mag_declination.h>
+      41             : 
+      42             : #include <mrs_uav_state_estimators/CorrectionConfig.h>
+      43             : 
+      44             : 
+      45             : namespace mrs_uav_state_estimators
+      46             : {
+      47             : 
+      48             : typedef enum
+      49             : {
+      50             :   LATERAL,
+      51             :   ALTITUDE,
+      52             :   HEADING,
+      53             :   LATALT,
+      54             : } EstimatorType_t;
+      55             : const int n_EstimatorType_t = 4;
+      56             : 
+      57             : typedef enum
+      58             : {
+      59             :   UNKNOWN,
+      60             :   ODOMETRY,
+      61             :   POSE,
+      62             :   POSECOV,
+      63             :   RANGE,
+      64             :   IMU,
+      65             :   RTK_GPS,
+      66             :   NAVSATFIX,
+      67             :   MAG_HDG,
+      68             :   MAG_FIELD,
+      69             :   POINT,
+      70             :   VECTOR,
+      71             :   QUAT,
+      72             : } MessageType_t;
+      73             : const int n_MessageType_t = 10;
+      74             : 
+      75             : const std::map<std::string, MessageType_t> map_msg_type{{"nav_msgs/Odometry", MessageType_t::ODOMETRY},
+      76             :                                                         {"geometry_msgs/PoseStamped", MessageType_t::POSE},
+      77             :                                                         {"geometry_msgs/PoseWithCovarianceStamped", MessageType_t::POSECOV},
+      78             :                                                         {"sensor_msgs/Range", MessageType_t::RANGE},
+      79             :                                                         {"sensor_msgs/Imu", MessageType_t::IMU},
+      80             :                                                         {"mrs_msgs/RtkGps", MessageType_t::RTK_GPS},
+      81             :                                                         {"sensor_msgs/NavSatFix", MessageType_t::NAVSATFIX},
+      82             :                                                         {"mrs_msgs/Float64Stamped", MessageType_t::MAG_HDG},
+      83             :                                                         {"sensor_msgs/MagneticField", MessageType_t::MAG_FIELD},
+      84             :                                                         {"geometry_msgs/PointStamped", MessageType_t::POINT},
+      85             :                                                         {"geometry_msgs/Vector3Stamped", MessageType_t::VECTOR},
+      86             :                                                         {"geometry_msgs/QuaternionStamped", MessageType_t::QUAT}};
+      87             : 
+      88             : template <int n_measurements>
+      89             : class Correction {
+      90             : 
+      91             :   using CommonHandlers_t  = mrs_uav_managers::estimation_manager::CommonHandlers_t;
+      92             :   using PrivateHandlers_t = mrs_uav_managers::estimation_manager::PrivateHandlers_t;
+      93             :   using StateId_t         = mrs_uav_managers::estimation_manager::StateId_t;
+      94             : 
+      95             : public:
+      96             :   typedef Eigen::Matrix<double, n_measurements, 1>         measurement_t;
+      97             :   typedef mrs_lib::DynamicReconfigureMgr<CorrectionConfig> drmgr_t;
+      98             : 
+      99             :   struct MeasurementStamped
+     100             :   {
+     101             :     ros::Time     stamp;
+     102             :     measurement_t value;
+     103             :   };
+     104             : 
+     105             : public:
+     106             :   Correction(ros::NodeHandle& nh, const std::string& est_name, const std::string& name, const std::string& frame_id, const EstimatorType_t& est_type,
+     107             :              const std::shared_ptr<CommonHandlers_t>& ch, const std::shared_ptr<PrivateHandlers_t>& ph, std::function<double(int, int)> fun_get_state,
+     108             :              std::function<void(MeasurementStamped, double, StateId_t)> fun_apply_correction);
+     109             : 
+     110             :   std::string getName() const;
+     111             :   std::string getNamespacedName() const;
+     112             :   std::string getPrintName() const;
+     113             : 
+     114             :   double    getR();
+     115             :   void      setR(const double R);
+     116             :   StateId_t getStateId() const;
+     117             : 
+     118             :   bool             isHealthy();
+     119             :   ros::Time        healthy_time_;
+     120             :   std::atomic_bool is_healthy_    = true;
+     121             :   std::atomic_bool is_delay_ok_   = true;
+     122             :   std::atomic_bool is_dt_ok_      = true;
+     123             :   std::atomic_bool is_nan_free_   = true;
+     124             :   std::atomic_bool got_first_msg_ = false;
+     125             : 
+     126             :   int counter_nan_ = 0;
+     127             : 
+     128             :   std::optional<MeasurementStamped> getRawCorrection();
+     129             :   std::optional<MeasurementStamped> getProcessedCorrection();
+     130             : 
+     131             :   void resetProcessors();
+     132             : 
+     133             : private:
+     134             :   std::atomic_bool is_initialized_ = false;
+     135             : 
+     136             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_odom_;
+     137             :   void                                          callbackOdometry(const nav_msgs::Odometry::ConstPtr msg);
+     138             :   std::optional<measurement_t>                  getCorrectionFromOdometry(const nav_msgs::OdometryConstPtr msg);
+     139             : 
+     140             :   mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped> sh_pose_s_;
+     141             :   void                                                  callbackPoseStamped(const geometry_msgs::PoseStamped::ConstPtr msg);
+     142             :   std::optional<measurement_t>                          getCorrectionFromPoseStamped(const geometry_msgs::PoseStampedConstPtr msg);
+     143             : 
+     144             :   mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped> sh_pose_wcs_;
+     145             : 
+     146             :   mrs_lib::SubscribeHandler<mrs_msgs::RtkGps> sh_rtk_;
+     147             :   void                                        callbackRtk(const mrs_msgs::RtkGps::ConstPtr msg);
+     148             :   std::optional<measurement_t>                getCorrectionFromRtk(const mrs_msgs::RtkGpsConstPtr msg);
+     149             : 
+     150             :   mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix> sh_navsatfix_;
+     151             :   void                                              callbackNavSatFix(const sensor_msgs::NavSatFix::ConstPtr msg);
+     152             :   std::optional<measurement_t>                      getCorrectionFromNavSatFix(const sensor_msgs::NavSatFixConstPtr msg);
+     153             : 
+     154             :   void   getAvgInitZ(const double z);
+     155             :   bool   got_avg_init_z_ = false;
+     156             :   double init_z_avg_     = 0.0;
+     157             :   int    got_z_counter_  = 0;
+     158             : 
+     159             :   mrs_lib::SubscribeHandler<geometry_msgs::PointStamped> sh_point_;
+     160             :   void                                                   callbackPoint(const geometry_msgs::PointStamped::ConstPtr msg);
+     161             :   std::optional<measurement_t>                           getCorrectionFromPoint(const geometry_msgs::PointStampedConstPtr msg);
+     162             : 
+     163             :   mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped> sh_vector_;
+     164             :   std::optional<measurement_t>                             getCorrectionFromVector(const geometry_msgs::Vector3StampedConstPtr msg);
+     165             :   void                                                     callbackVector(const geometry_msgs::Vector3Stamped::ConstPtr msg);
+     166             : 
+     167             :   mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_orientation_;  // for obtaining heading rate
+     168             :   std::string                                                 orientation_topic_;
+     169             : 
+     170             :   mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_quat_;
+     171             :   measurement_t                                               prev_hdg_measurement_;
+     172             :   bool                                                        got_first_hdg_measurement_ = false;
+     173             :   bool                                                        init_hdg_in_zero_          = false;
+     174             :   double                                                      first_hdg_measurement_     = 0.0;
+     175             : 
+     176             :   mrs_lib::SubscribeHandler<sensor_msgs::Imu> sh_imu_;
+     177             :   std::optional<measurement_t>                getCorrectionFromImu(const sensor_msgs::ImuConstPtr msg);
+     178             :   void                                        callbackImu(const sensor_msgs::Imu::ConstPtr msg);
+     179             : 
+     180             :   mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped> sh_mag_hdg_;
+     181             :   std::optional<measurement_t>                        getCorrectionFromMagHeading(const mrs_msgs::Float64StampedConstPtr msg);
+     182             :   void                                                callbackMagHeading(const mrs_msgs::Float64Stamped::ConstPtr msg);
+     183             :   double                                              mag_hdg_previous_;
+     184             :   bool                                                got_first_mag_hdg_;
+     185             : 
+     186             :   mrs_lib::SubscribeHandler<sensor_msgs::MagneticField> sh_mag_field_;
+     187             :   std::optional<measurement_t>                          getCorrectionFromMagField(const sensor_msgs::MagneticFieldConstPtr msg);
+     188             :   void                                                  callbackMagField(const sensor_msgs::MagneticField::ConstPtr msg);
+     189             : 
+     190             :   mrs_lib::SubscribeHandler<sensor_msgs::Range> sh_range_;
+     191             :   std::optional<measurement_t>                  getCorrectionFromRange(const sensor_msgs::RangeConstPtr msg);
+     192             :   void                                          callbackRange(const sensor_msgs::Range::ConstPtr msg);
+     193             :   ros::ServiceServer                            ser_toggle_range_;
+     194             :   bool                                          callbackToggleRange(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     195             :   bool                                          range_enabled_ = true;
+     196             : 
+     197             :   void applyCorrection(const measurement_t& meas, const ros::Time& stamp);
+     198             : 
+     199             :   mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection> ph_correction_raw_;
+     200             :   mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection> ph_correction_proc_;
+     201             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>      ph_delay_;
+     202             :   mrs_lib::PublisherHandler<geometry_msgs::PointStamped>   ph_mag_field_untilted_;
+     203             : 
+     204             :   const std::string                  est_name_;
+     205             :   const std::string                  name_;
+     206             :   const std::string                  ns_frame_id_;
+     207             :   const EstimatorType_t              est_type_;
+     208             :   std::shared_ptr<CommonHandlers_t>  ch_;
+     209             :   std::shared_ptr<PrivateHandlers_t> ph_;
+     210             : 
+     211             :   MessageType_t msg_type_;
+     212             :   std::string   msg_topic_;
+     213             :   double        msg_timeout_;
+     214             : 
+     215             :   double     R_;
+     216             :   double     default_R_;
+     217             :   double     R_coeff_;
+     218             :   std::mutex mtx_R_;
+     219             :   StateId_t  state_id_;
+     220             :   bool       is_in_body_frame_ = true;
+     221             :   double     gravity_norm_     = 9.8066;
+     222             : 
+     223             :   bool        transform_to_frame_enabled_ = false;
+     224             :   std::string transform_to_frame_;
+     225             :   std::string transform_from_frame_;
+     226             : 
+     227             :   std::unique_ptr<drmgr_t> drmgr_;
+     228             : 
+     229             :   std::optional<measurement_t> getCorrectionFromQuat(const geometry_msgs::QuaternionStampedConstPtr msg);
+     230             :   std::optional<measurement_t> getZVelUntilted(const geometry_msgs::Vector3& msg, const std_msgs::Header& header);
+     231             :   std::optional<measurement_t> getVecInFrame(const geometry_msgs::Vector3& vec_in, const std_msgs::Header& source_header, const std::string target_frame);
+     232             :   std::optional<geometry_msgs::Vector3> transformVecToFrame(const geometry_msgs::Vector3& vec_in, const std_msgs::Header& source_header,
+     233             :                                                             const std::string target_frame);
+     234             :   std::optional<geometry_msgs::Point>   getInFrame(const geometry_msgs::Point& vec_in, const std_msgs::Header& source_header, const std::string target_frame);
+     235             : 
+     236             :   std::optional<geometry_msgs::Pose> transformRtkToFcu(const geometry_msgs::PoseStamped& pose_in) const;
+     237             : 
+     238             :   void   checkMsgDelay(const ros::Time& msg_time);
+     239             :   double msg_delay_limit_;
+     240             :   double msg_delay_warn_limit_;
+     241             : 
+     242             :   double time_since_last_msg_limit_;
+     243             : 
+     244             :   std::shared_ptr<Processor<n_measurements>> createProcessorFromName(const std::string& name, ros::NodeHandle& nh);
+     245             :   bool                                       process(measurement_t& measurement);
+     246             : 
+     247             :   bool             isTimestampOk();
+     248             :   bool             isMsgComing();
+     249             :   std::atomic_bool first_timestamp_ = true;
+     250             :   ros::Time        msg_time_;
+     251             :   ros::Time        prev_msg_time_;
+     252             :   std::mutex       mtx_msg_time_;
+     253             : 
+     254             :   std::vector<std::string>                                                    processor_names_;
+     255             :   std::unordered_map<std::string, std::shared_ptr<Processor<n_measurements>>> processors_;
+     256             : 
+     257             :   std::function<double(int, int)>                            fun_get_state_;
+     258             :   std::function<void(MeasurementStamped, double, StateId_t)> fun_apply_correction_;
+     259             : 
+     260             :   void publishCorrection(const MeasurementStamped& measurement_stamped, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection>& ph_corr);
+     261             :   void publishDelay(const double delay);
+     262             : };
+     263             : 
+     264             : /*//{ constructor */
+     265             : template <int n_measurements>
+     266         402 : Correction<n_measurements>::Correction(ros::NodeHandle& nh, const std::string& est_name, const std::string& name, const std::string& ns_frame_id,
+     267             :                                        const EstimatorType_t& est_type, const std::shared_ptr<CommonHandlers_t>& ch,
+     268             :                                        const std::shared_ptr<PrivateHandlers_t>& ph, std::function<double(int, int)> fun_get_state,
+     269             :                                        std::function<void(MeasurementStamped, double, StateId_t)> fun_apply_correction)
+     270             :     : est_name_(est_name),
+     271             :       name_(name),
+     272             :       ns_frame_id_(ns_frame_id),
+     273             :       est_type_(est_type),
+     274             :       ch_(ch),
+     275             :       ph_(ph),
+     276             :       fun_get_state_(fun_get_state),
+     277         402 :       fun_apply_correction_(fun_apply_correction) {
+     278             : 
+     279             :   // | --------------------- load parameters -------------------- |
+     280             : 
+     281         804 :   std::string msg_type_string;
+     282             : 
+     283         402 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + getNamespacedName() + "/");
+     284             : 
+     285         402 :   ph->param_loader->loadParam("message/type", msg_type_string);
+     286         402 :   if (map_msg_type.find(msg_type_string) == map_msg_type.end()) {
+     287           0 :     ROS_ERROR("[%s]: wrong message type: %s of correction %s", getPrintName().c_str(), msg_type_string.c_str(), getName().c_str());
+     288           0 :     ros::shutdown();
+     289             :   }
+     290         402 :   msg_type_ = map_msg_type.at(msg_type_string);
+     291             : 
+     292         402 :   ph->param_loader->loadParam("message/topic", msg_topic_);
+     293         402 :   msg_topic_ = "/" + ch_->uav_name + "/" + msg_topic_;
+     294         402 :   ph->param_loader->loadParam("message/limit/delay", msg_delay_limit_);
+     295         402 :   msg_delay_warn_limit_ = msg_delay_limit_ / 2;  // maybe specify this as a param?
+     296         402 :   ph->param_loader->loadParam("message/limit/time_since_last", time_since_last_msg_limit_);
+     297             : 
+     298             :   int state_id_tmp;
+     299         402 :   ph->param_loader->loadParam("state_id", state_id_tmp);
+     300         402 :   if (state_id_tmp < n_StateId_t) {
+     301         402 :     state_id_ = static_cast<StateId_t>(state_id_tmp);
+     302             :   } else {
+     303           0 :     ROS_ERROR("[%s]: wrong state id: %d of correction %s", getPrintName().c_str(), state_id_tmp, getName().c_str());
+     304           0 :     ros::shutdown();
+     305             :   }
+     306             : 
+     307         402 :   ph->param_loader->loadParam("transform/enabled", transform_to_frame_enabled_, false);
+     308         402 :   if (transform_to_frame_enabled_) {
+     309           0 :     ph->param_loader->loadParam("transform/from_frame", transform_from_frame_);
+     310           0 :     transform_from_frame_ = ch_->uav_name + "/" + transform_from_frame_;
+     311           0 :     ph->param_loader->loadParam("transform/to_frame", transform_to_frame_);
+     312           0 :     transform_to_frame_ = ch_->uav_name + "/" + transform_to_frame_;
+     313             :   }
+     314             : 
+     315             : 
+     316         402 :   if (state_id_ == StateId_t::VELOCITY) {
+     317         120 :     ph->param_loader->loadParam("body_frame", is_in_body_frame_, true);
+     318             :   }
+     319             : 
+     320         402 :   if (state_id_ == StateId_t::ACCELERATION) {
+     321           0 :     ph->param_loader->loadParam("body_frame", is_in_body_frame_, true);
+     322           0 :     ph->param_loader->loadParam("gravity_norm", gravity_norm_, 9.8066);
+     323             :   }
+     324             : 
+     325         402 :   ph->param_loader->loadParam("noise", R_);
+     326         402 :   ph->param_loader->loadParam("noise_unhealthy_coeff", R_coeff_);
+     327         402 :   default_R_ = R_;
+     328             : 
+     329             :   // | --------------- processors initialization --------------- |
+     330         402 :   ph->param_loader->loadParam("processors", processor_names_);
+     331             : 
+     332         772 :   for (auto proc_name : processor_names_) {
+     333         370 :     processors_[proc_name] = createProcessorFromName(proc_name, nh);
+     334             :   }
+     335             : 
+     336             :   // | ------------- initialize dynamic reconfigure ------------- |
+     337         402 :   drmgr_               = std::make_unique<drmgr_t>(ros::NodeHandle("~/" + getNamespacedName()), getPrintName());
+     338         402 :   drmgr_->config.noise = R_;
+     339         402 :   drmgr_->update_config(drmgr_->config);
+     340             : 
+     341             :   // | -------------- initialize subscribe handlers ------------- |
+     342         804 :   mrs_lib::SubscribeHandlerOptions shopts;
+     343         402 :   shopts.nh                 = nh;
+     344         402 :   shopts.node_name          = getPrintName();
+     345         402 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     346         402 :   shopts.threadsafe         = true;
+     347         402 :   shopts.autostart          = true;
+     348         402 :   shopts.queue_size         = 10;
+     349         402 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     350             : 
+     351         402 :   switch (msg_type_) {
+     352           0 :     case MessageType_t::ODOMETRY: {
+     353           0 :       sh_odom_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, msg_topic_, &Correction::callbackOdometry, this);
+     354           0 :       break;
+     355             :     }
+     356           0 :     case MessageType_t::POSE: {
+     357           0 :       sh_pose_s_ = mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped>(shopts, msg_topic_, &Correction::callbackPoseStamped, this);
+     358           0 :       break;
+     359             :     }
+     360           0 :     case MessageType_t::POSECOV: {
+     361             :       // TODO implement
+     362             :       /* sh_pose_wcs_ = mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped>(shopts, msg_topic_); */
+     363           0 :       break;
+     364             :     }
+     365         164 :     case MessageType_t::RANGE: {
+     366         164 :       sh_range_                   = mrs_lib::SubscribeHandler<sensor_msgs::Range>(shopts, msg_topic_, &Correction::callbackRange, this);
+     367         164 :       const std::size_t found     = ros::this_node::getName().find_last_of("/");
+     368         328 :       std::string       node_name = ros::this_node::getName().substr(found + 1);
+     369         328 :       ser_toggle_range_ =
+     370         164 :           nh.advertiseService(ros::this_node::getName() + "/" + getNamespacedName() + "/toggle_range_in", &Correction::callbackToggleRange, this);
+     371         164 :       break;
+     372             :     }
+     373           0 :     case MessageType_t::IMU: {
+     374           0 :       sh_imu_ = mrs_lib::SubscribeHandler<sensor_msgs::Imu>(shopts, msg_topic_, &Correction::callbackImu, this);
+     375           0 :       break;
+     376             :     }
+     377           6 :     case MessageType_t::RTK_GPS: {
+     378           6 :       sh_rtk_ = mrs_lib::SubscribeHandler<mrs_msgs::RtkGps>(shopts, msg_topic_, &Correction::callbackRtk, this);
+     379           6 :       break;
+     380             :     }
+     381           0 :     case MessageType_t::NAVSATFIX: {
+     382           0 :       sh_navsatfix_ = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, msg_topic_, &Correction::callbackNavSatFix, this);
+     383           0 :       break;
+     384             :     }
+     385           0 :     case MessageType_t::MAG_HDG: {
+     386           0 :       sh_mag_hdg_ = mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>(shopts, msg_topic_, &Correction::callbackMagHeading, this);
+     387           0 :       break;
+     388             :     }
+     389           0 :     case MessageType_t::MAG_FIELD: {
+     390           0 :       sh_mag_field_ = mrs_lib::SubscribeHandler<sensor_msgs::MagneticField>(shopts, msg_topic_, &Correction::callbackMagField, this);
+     391           0 :       ph_mag_field_untilted_ = mrs_lib::PublisherHandler<geometry_msgs::PointStamped>(nh, est_name_ + "/correction/" + getName() + "/fcu_untilted", 10);
+     392             : 
+     393           0 :       break;
+     394             :     }
+     395         112 :     case MessageType_t::POINT: {
+     396         112 :       sh_point_ = mrs_lib::SubscribeHandler<geometry_msgs::PointStamped>(shopts, msg_topic_, &Correction::callbackPoint, this);
+     397         112 :       break;
+     398             :     }
+     399         120 :     case MessageType_t::VECTOR: {
+     400         120 :       sh_vector_ = mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped>(shopts, msg_topic_, &Correction::callbackVector, this);
+     401         120 :       break;
+     402             :     }
+     403           0 :     case MessageType_t::QUAT: {
+     404           0 :       sh_quat_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, msg_topic_);
+     405           0 :       break;
+     406             :     }
+     407           0 :     case MessageType_t::UNKNOWN: {
+     408           0 :       ROS_ERROR("[%s]: UNKNOWN message type of correction", getPrintName().c_str());
+     409           0 :       break;
+     410             :     }
+     411           0 :     default: {
+     412           0 :       ROS_ERROR("[%s]: unhandled message type", getPrintName().c_str());
+     413             :     }
+     414             :   }
+     415             : 
+     416             :   // | ------ subscribe orientation for obtaingin hdg rate ------ |
+     417             :   /* if (est_type_ == EstimatorType_t::HEADING && state_id_ == StateId_t::VELOCITY) { */
+     418             :   /*   ph->param_loader->loadParam("message/orientation_topic", orientation_topic_); */
+     419             :   /*   orientation_topic_ = "/" + ch_->uav_name + "/" + orientation_topic_; */
+     420             :   /*   sh_orientation_    = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, orientation_topic_); */
+     421             :   /* } */
+     422             : 
+     423         402 :   if (est_type_ == EstimatorType_t::HEADING) {
+     424           0 :     ph->param_loader->loadParam("init_hdg_in_zero", init_hdg_in_zero_, false);
+     425             :   }
+     426             : 
+     427             : 
+     428             :   // | --------------- initialize publish handlers -------------- |
+     429         402 :   if (ch_->debug_topics.correction) {
+     430         402 :     ph_correction_raw_  = mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection>(nh, est_name_ + "/correction/" + getName() + "/raw", 10);
+     431         402 :     ph_correction_proc_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection>(nh, est_name_ + "/correction/" + getName() + "/proc", 10);
+     432             :   }
+     433         402 :   if (ch_->debug_topics.corr_delay) {
+     434           0 :     ph_delay_ = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh, est_name_ + "/correction/" + getName() + "/delay", 10);
+     435             :   }
+     436             : 
+     437             :   // | --- check whether all parameters were loaded correctly --- |
+     438         402 :   if (!ph->param_loader->loadedSuccessfully()) {
+     439           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
+     440           0 :     ros::shutdown();
+     441             :   }
+     442             : 
+     443         402 :   healthy_time_ = ros::Time(0);
+     444             : 
+     445         402 :   is_initialized_ = true;
+     446         402 : }
+     447             : /*//}*/
+     448             : 
+     449             : /*//{ getName() */
+     450             : template <int n_measurements>
+     451         804 : std::string Correction<n_measurements>::getName() const {
+     452         804 :   return name_;
+     453             : }
+     454             : /*//}*/
+     455             : 
+     456             : /*//{ getNamespacedName() */
+     457             : template <int n_measurements>
+     458        1623 : std::string Correction<n_measurements>::getNamespacedName() const {
+     459        1623 :   return est_name_ + "/" + name_;
+     460             : }
+     461             : /*//}*/
+     462             : 
+     463             : /*//{ getPrintName() */
+     464             : template <int n_measurements>
+     465         863 : std::string Correction<n_measurements>::getPrintName() const {
+     466         863 :   return ch_->nodelet_name + "/" + est_name_ + "/" + name_;
+     467             : }
+     468             : /*//}*/
+     469             : 
+     470             : /*//{ getR() */
+     471             : template <int n_measurements>
+     472     2647174 : double Correction<n_measurements>::getR() {
+     473     2647174 :   std::scoped_lock lock(mtx_R_);
+     474     2646545 :   default_R_ = drmgr_->config.noise;
+     475     5292566 :   return R_;
+     476             : }
+     477             : /*//}*/
+     478             : 
+     479             : /*//{ setR() */
+     480             : template <int n_measurements>
+     481      721583 : void Correction<n_measurements>::setR(const double R) {
+     482      721583 :   std::scoped_lock lock(mtx_R_);
+     483      721520 :   R_ = R;
+     484      721564 : }
+     485             : /*//}*/
+     486             : 
+     487             : /*//{ getStateId() */
+     488             : template <int n_measurements>
+     489      721947 : StateId_t Correction<n_measurements>::getStateId() const {
+     490      721947 :   return state_id_;
+     491             : }
+     492             : /*//}*/
+     493             : 
+     494             : /*//{ isHealthy() */
+     495             : template <int n_measurements>
+     496      709524 : bool Correction<n_measurements>::isHealthy() {
+     497             : 
+     498      709524 :   if (!is_initialized_) {
+     499           0 :     return false;
+     500             :   }
+     501             : 
+     502      709491 :   is_dt_ok_ = isMsgComing();
+     503             : 
+     504      709542 :   if (!is_delay_ok_) {
+     505           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: delay not ok", getPrintName().c_str());
+     506             :   }
+     507             : 
+     508      709516 :   if (!is_healthy_) {
+     509           0 :     if (is_dt_ok_ && is_delay_ok_) {
+     510           0 :       if (healthy_time_ > ros::Time(10)) {
+     511           0 :         is_healthy_ = true;
+     512             :       }
+     513             :     } else {
+     514           0 :       healthy_time_ = ros::Time(0);
+     515             :     }
+     516             :   }
+     517             : 
+     518      709513 :   is_healthy_ = is_healthy_ && is_dt_ok_ && is_delay_ok_;
+     519             : 
+     520      709558 :   return is_healthy_;
+     521             : }
+     522             : /*//}*/
+     523             : 
+     524             : /*//{ getRawCorrection() */
+     525             : template <int n_measurements>
+     526       16886 : std::optional<typename Correction<n_measurements>::MeasurementStamped> Correction<n_measurements>::getRawCorrection() {
+     527             : 
+     528       16886 :   if (!is_initialized_) {
+     529           0 :     return {};
+     530             :   }
+     531             : 
+     532       16878 :   MeasurementStamped measurement_stamped;
+     533             : 
+     534       16878 :   switch (msg_type_) {
+     535             : 
+     536           0 :     case MessageType_t::ODOMETRY: {
+     537             : 
+     538           0 :       if (!sh_odom_.hasMsg()) {
+     539           0 :         return {};
+     540             :       }
+     541             : 
+     542           0 :       auto msg                  = sh_odom_.getMsg();
+     543           0 :       measurement_stamped.stamp = msg->header.stamp;
+     544             :       /* if (!isTimestampOk(measurement_stamped.stamp)) { */
+     545             :       /*   return {}; */
+     546             :       /* } */
+     547             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     548             : 
+     549             :       /* if (!is_delay_ok_) { */
+     550             :       /*   return {}; */
+     551             :       /* } */
+     552           0 :       auto res = getCorrectionFromOdometry(msg);
+     553           0 :       if (res) {
+     554           0 :         measurement_stamped.value = res.value();
+     555             :       } else {
+     556           0 :         return {};
+     557             :       }
+     558           0 :       break;
+     559             :     }
+     560             : 
+     561           0 :     case MessageType_t::POSE: {
+     562             : 
+     563           0 :       if (!sh_pose_s_.hasMsg()) {
+     564           0 :         return {};
+     565             :       }
+     566             : 
+     567           0 :       auto msg                  = sh_pose_s_.getMsg();
+     568           0 :       measurement_stamped.stamp = msg->header.stamp;
+     569             :       /* if (!isTimestampOk(measurement_stamped.stamp)) { */
+     570             :       /*   return {}; */
+     571             :       /* } */
+     572             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     573             : 
+     574             :       /* if (!is_delay_ok_) { */
+     575             :       /*   return {}; */
+     576             :       /* } */
+     577           0 :       auto res = getCorrectionFromPoseStamped(msg);
+     578           0 :       if (res) {
+     579           0 :         measurement_stamped.value = res.value();
+     580             :       } else {
+     581           0 :         return {};
+     582             :       }
+     583           0 :       break;
+     584             :     }
+     585             : 
+     586           0 :     case MessageType_t::POSECOV: {
+     587             :       // TODO implement
+     588             :       /* return getCorrectionFromPoseWCS(msg); */
+     589           0 :       is_healthy_ = false;
+     590           0 :       return {};
+     591             :       break;
+     592             :     }
+     593             : 
+     594        6873 :     case MessageType_t::RANGE: {
+     595             : 
+     596        6873 :       if (!range_enabled_) {
+     597           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: fusing range corrections is disabled", getPrintName().c_str());
+     598        2741 :         return {};
+     599             :       }
+     600             : 
+     601        6873 :       if (!sh_range_.hasMsg()) {
+     602        2562 :         return {};
+     603             :       }
+     604             : 
+     605        4310 :       auto msg                  = sh_range_.getMsg();
+     606        4308 :       measurement_stamped.stamp = msg->header.stamp;
+     607             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     608             : 
+     609             :       /* if (!is_delay_ok_) { */
+     610             :       /*   return {}; */
+     611             :       /* } */
+     612             : 
+     613        4305 :       auto res = getCorrectionFromRange(msg);
+     614        4309 :       if (res) {
+     615        4135 :         measurement_stamped.value = res.value();
+     616             :       } else {
+     617         174 :         return {};
+     618             :       }
+     619        4135 :       break;
+     620             :     }
+     621             : 
+     622           0 :     case MessageType_t::IMU: {
+     623             : 
+     624           0 :       if (!sh_imu_.hasMsg()) {
+     625           0 :         ROS_ERROR_THROTTLE(1.0, " no imu msgs so far");
+     626           0 :         return {};
+     627             :       }
+     628             : 
+     629           0 :       auto msg                  = sh_imu_.getMsg();
+     630           0 :       measurement_stamped.stamp = msg->header.stamp;
+     631             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     632             : 
+     633             :       /* if (!is_delay_ok_) { */
+     634             :       /*   ROS_ERROR("[%s]: rtk msg delay not ok", ros::this_node::getName().c_str()); */
+     635             :       /*   return {}; */
+     636             :       /* } */
+     637             : 
+     638           0 :       auto res = getCorrectionFromImu(msg);
+     639           0 :       if (res) {
+     640           0 :         measurement_stamped.value = res.value();
+     641             :       } else {
+     642           0 :         ROS_ERROR_THROTTLE(1.0, "[%s]: could not get imu correction", ros::this_node::getName().c_str());
+     643           0 :         return {};
+     644             :       }
+     645             : 
+     646           0 :       break;
+     647             :     }
+     648             : 
+     649         104 :     case MessageType_t::RTK_GPS: {
+     650             : 
+     651         104 :       if (!sh_rtk_.hasMsg()) {
+     652           0 :         ROS_ERROR_THROTTLE(1.0, " no rtk msgs so far");
+     653          15 :         return {};
+     654             :       }
+     655             : 
+     656         104 :       auto msg                  = sh_rtk_.getMsg();
+     657         104 :       measurement_stamped.stamp = msg->header.stamp;
+     658             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     659             : 
+     660             :       /* if (!is_delay_ok_) { */
+     661             :       /*   ROS_ERROR("[%s]: rtk msg delay not ok", ros::this_node::getName().c_str()); */
+     662             :       /*   return {}; */
+     663             :       /* } */
+     664             : 
+     665         104 :       auto res = getCorrectionFromRtk(msg);
+     666         104 :       if (res) {
+     667          89 :         measurement_stamped.value = res.value();
+     668             :       } else {
+     669          15 :         ROS_ERROR_THROTTLE(1.0, "[%s]: could not get rtk correction", ros::this_node::getName().c_str());
+     670          15 :         return {};
+     671             :       }
+     672             : 
+     673          89 :       break;
+     674             :     }
+     675             : 
+     676           0 :     case MessageType_t::NAVSATFIX: {
+     677             : 
+     678           0 :       if (!sh_navsatfix_.hasMsg()) {
+     679           0 :         ROS_ERROR_THROTTLE(1.0, " no navsatfix msgs so far");
+     680           0 :         return {};
+     681             :       }
+     682             : 
+     683           0 :       auto msg                  = sh_navsatfix_.getMsg();
+     684           0 :       measurement_stamped.stamp = msg->header.stamp;
+     685             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     686             : 
+     687             :       /* if (!is_delay_ok_) { */
+     688             :       /*   ROS_ERROR("[%s]: rtk msg delay not ok", ros::this_node::getName().c_str()); */
+     689             :       /*   return {}; */
+     690             :       /* } */
+     691             : 
+     692           0 :       auto res = getCorrectionFromNavSatFix(msg);
+     693           0 :       if (res) {
+     694           0 :         measurement_stamped.value = res.value();
+     695             :       } else {
+     696           0 :         ROS_ERROR_THROTTLE(1.0, "[%s]: could not get navsatfix correction", ros::this_node::getName().c_str());
+     697           0 :         return {};
+     698             :       }
+     699             : 
+     700           0 :       break;
+     701             :     }
+     702             : 
+     703           0 :     case MessageType_t::MAG_HDG: {
+     704             : 
+     705           0 :       if (!sh_mag_hdg_.hasMsg()) {
+     706           0 :         return {};
+     707             :       }
+     708             : 
+     709           0 :       auto msg                  = sh_mag_hdg_.getMsg();
+     710           0 :       measurement_stamped.stamp = msg->header.stamp;
+     711             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     712             : 
+     713             :       /* if (!is_delay_ok_) { */
+     714             :       /*   return {}; */
+     715             :       /* } */
+     716           0 :       auto res = getCorrectionFromMagHeading(msg);
+     717           0 :       if (res) {
+     718           0 :         measurement_stamped.value = res.value();
+     719             :       } else {
+     720           0 :         return {};
+     721             :       }
+     722           0 :       break;
+     723             :     }
+     724             : 
+     725           0 :     case MessageType_t::MAG_FIELD: {
+     726             : 
+     727           0 :       if (!sh_mag_field_.hasMsg()) {
+     728           0 :         return {};
+     729             :       }
+     730             : 
+     731           0 :       auto msg                  = sh_mag_field_.getMsg();
+     732           0 :       measurement_stamped.stamp = msg->header.stamp;
+     733             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     734             : 
+     735             :       /* if (!is_delay_ok_) { */
+     736             :       /*   return {}; */
+     737             :       /* } */
+     738           0 :       auto res = getCorrectionFromMagField(msg);
+     739           0 :       if (res) {
+     740           0 :         measurement_stamped.value = res.value();
+     741             :       } else {
+     742           0 :         return {};
+     743             :       }
+     744           0 :       break;
+     745             :     }
+     746             : 
+     747        7792 :     case MessageType_t::POINT: {
+     748             : 
+     749        7792 :       if (!sh_point_.hasMsg()) {
+     750        3736 :         return {};
+     751             :       }
+     752             : 
+     753        4056 :       auto msg                  = sh_point_.getMsg();
+     754        4056 :       measurement_stamped.stamp = msg->header.stamp;
+     755             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     756             : 
+     757             :       /* if (!is_delay_ok_) { */
+     758             :       /*   return {}; */
+     759             :       /* } */
+     760        4056 :       auto res = getCorrectionFromPoint(msg);
+     761        4056 :       if (res) {
+     762        4056 :         measurement_stamped.value = res.value();
+     763             :       } else {
+     764           0 :         return {};
+     765             :       }
+     766        4056 :       break;
+     767             :     }
+     768             : 
+     769        2116 :     case MessageType_t::VECTOR: {
+     770             : 
+     771        2116 :       if (!sh_vector_.hasMsg()) {
+     772        1996 :         return {};
+     773             :       }
+     774             : 
+     775         145 :       auto msg                  = sh_vector_.getMsg();
+     776         145 :       measurement_stamped.stamp = msg->header.stamp;
+     777             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     778             : 
+     779             :       /* if (!is_delay_ok_) { */
+     780             :       /*   return {}; */
+     781             :       /* } */
+     782         145 :       auto res = getCorrectionFromVector(msg);
+     783         145 :       if (res) {
+     784         120 :         measurement_stamped.value = res.value();
+     785             :       } else {
+     786          25 :         return {};
+     787             :       }
+     788         120 :       break;
+     789             :     }
+     790             : 
+     791           0 :     case MessageType_t::QUAT: {
+     792             : 
+     793           0 :       if (!sh_quat_.newMsg()) {
+     794           0 :         return {};
+     795             :       }
+     796             : 
+     797           0 :       auto msg                  = sh_quat_.getMsg();
+     798           0 :       measurement_stamped.stamp = msg->header.stamp;
+     799             :       /* checkMsgDelay(measurement_stamped.stamp); */
+     800             : 
+     801             :       /* if (!is_delay_ok_) { */
+     802             :       /*   return {}; */
+     803             :       /* } */
+     804           0 :       auto res = getCorrectionFromQuat(msg);
+     805           0 :       if (res) {
+     806           0 :         measurement_stamped.value = res.value();
+     807             :       } else {
+     808           0 :         return {};
+     809             :       }
+     810           0 :       break;
+     811             :     }
+     812             : 
+     813           0 :     default: {
+     814           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: this type of correction is not implemented in getCorrectionFromMessage()", getPrintName().c_str());
+     815           0 :       is_healthy_ = false;
+     816           0 :       return {};
+     817             :     }
+     818             :   }
+     819             : 
+     820             :   // check for nans
+     821        8400 :   is_nan_free_ = true;
+     822       20932 :   for (int i = 0; i < measurement_stamped.value.rows(); i++) {
+     823       12532 :     if (!std::isfinite(measurement_stamped.value(i))) {
+     824           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in correction. Total NaNs: %d", getPrintName().c_str(), ++counter_nan_);
+     825           0 :       is_nan_free_ = false;
+     826           0 :       return {};
+     827             :     }
+     828             :   }
+     829             : 
+     830        8400 :   got_first_msg_ = true;
+     831        8400 :   publishCorrection(measurement_stamped, ph_correction_raw_);
+     832             : 
+     833        8400 :   return measurement_stamped;
+     834             : }
+     835             : /*//}*/
+     836             : 
+     837             : /*//{ getProcessedCorrection() */
+     838             : template <int n_measurements>
+     839       16888 : std::optional<typename Correction<n_measurements>::MeasurementStamped> Correction<n_measurements>::getProcessedCorrection() {
+     840             : 
+     841       16888 :   MeasurementStamped measurement_stamped;
+     842       16882 :   auto               res = getRawCorrection();
+     843       16883 :   if (res) {
+     844        8399 :     MeasurementStamped measurement_stamped = res.value();
+     845        8399 :     if (process(measurement_stamped.value)) {
+     846         485 :       publishCorrection(measurement_stamped, ph_correction_proc_);
+     847         485 :       return measurement_stamped;
+     848             :     } else {
+     849        7915 :       return {};  // invalid correction
+     850             :     }
+     851             :   } else {
+     852        8483 :     return {};  // invalid correction
+     853             :   }
+     854             : }  // namespace mrs_uav_state_estimation
+     855             : /*//}*/
+     856             : 
+     857             : /*//{ callbackOdometry() */
+     858             : template <int n_measurements>
+     859           0 : void Correction<n_measurements>::callbackOdometry(const nav_msgs::Odometry::ConstPtr msg) {
+     860             : 
+     861           0 :   if (!is_initialized_) {
+     862           0 :     return;
+     863             :   }
+     864             : 
+     865           0 :   auto res = getCorrectionFromOdometry(msg);
+     866           0 :   if (res) {
+     867           0 :     applyCorrection(res.value(), msg->header.stamp);
+     868             :   }
+     869             : }
+     870             : /*//}*/
+     871             : 
+     872             : /*//{ getCorrectionFromOdometry() */
+     873             : template <int n_measurements>
+     874           0 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromOdometry(const nav_msgs::OdometryConstPtr msg) {
+     875             : 
+     876           0 :   switch (est_type_) {
+     877             : 
+     878             :     // handle lateral estimators
+     879           0 :     case EstimatorType_t::LATERAL: {
+     880             : 
+     881           0 :       switch (state_id_) {
+     882             : 
+     883           0 :         case StateId_t::POSITION: {
+     884           0 :           measurement_t measurement;
+     885           0 :           if (transform_to_frame_enabled_) {
+     886           0 :             std_msgs::Header header = msg->header;
+     887           0 :             header.frame_id         = transform_from_frame_;
+     888           0 :             auto res                = getInFrame(msg->pose.pose.position, header, transform_to_frame_);
+     889           0 :             if (res) {
+     890           0 :               measurement_t measurement;
+     891           0 :               measurement(0) = res.value().x;
+     892           0 :               measurement(1) = res.value().y;
+     893           0 :               return measurement;
+     894             :             } else {
+     895           0 :               ROS_WARN_THROTTLE(1.0, "[%s]: could not transform vel from odom", ros::this_node::getName().c_str());
+     896           0 :               return {};
+     897             :             }
+     898             : 
+     899             :           } else {
+     900           0 :             measurement(0) = msg->pose.pose.position.x;
+     901           0 :             measurement(1) = msg->pose.pose.position.y;
+     902             :           }
+     903           0 :           return measurement;
+     904             :           break;
+     905             :         }
+     906             : 
+     907           0 :         case StateId_t::VELOCITY: {
+     908           0 :           if (is_in_body_frame_) {
+     909           0 :             std_msgs::Header header = msg->header;
+     910           0 :             header.frame_id         = ch_->frames.ns_fcu;  // message in odometry is published in body frame
+     911           0 :             auto res                = getVecInFrame(msg->twist.twist.linear, header, ns_frame_id_ + "_att_only");
+     912           0 :             if (res) {
+     913           0 :               measurement_t measurement;
+     914           0 :               measurement = res.value();
+     915           0 :               return measurement;
+     916             :             } else {
+     917           0 :               ROS_WARN_THROTTLE(1.0, "[%s]: could not transform vel from odom", ros::this_node::getName().c_str());
+     918           0 :               return {};
+     919             :             }
+     920             :           } else {
+     921           0 :             measurement_t measurement;
+     922           0 :             measurement(0) = msg->twist.twist.linear.x;
+     923           0 :             measurement(1) = msg->twist.twist.linear.y;
+     924           0 :             return measurement;
+     925             :           }
+     926             :           break;
+     927             :         }
+     928             : 
+     929           0 :         default: {
+     930           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str());
+     931           0 :           return {};
+     932             :         }
+     933             :       }
+     934             :       break;
+     935             :     }
+     936             : 
+     937             :     // handle altitude estimators
+     938           0 :     case EstimatorType_t::ALTITUDE: {
+     939             : 
+     940           0 :       switch (state_id_) {
+     941             : 
+     942           0 :         case StateId_t::POSITION: {
+     943           0 :           measurement_t measurement;
+     944           0 :           if (transform_to_frame_enabled_) {
+     945           0 :             std_msgs::Header header = msg->header;
+     946           0 :             header.frame_id         = transform_from_frame_;
+     947           0 :             auto res                = getInFrame(msg->pose.pose.position, header, transform_to_frame_);
+     948           0 :             if (res) {
+     949           0 :               measurement_t measurement;
+     950           0 :               measurement(0) = res.value().z;
+     951           0 :               return measurement;
+     952             :             } else {
+     953           0 :               ROS_WARN_THROTTLE(1.0, "[%s]: could not transform vel from odom", ros::this_node::getName().c_str());
+     954           0 :               return {};
+     955             :             }
+     956             : 
+     957             :           } else {
+     958           0 :             measurement(0) = msg->pose.pose.position.z;
+     959             :           }
+     960           0 :           return measurement;
+     961             :           break;
+     962             :         }
+     963             : 
+     964           0 :         case StateId_t::VELOCITY: {
+     965           0 :           if (is_in_body_frame_) {
+     966           0 :             std_msgs::Header header = msg->header;
+     967           0 :             header.frame_id         = ch_->frames.ns_fcu;
+     968           0 :             auto res                = getZVelUntilted(msg->twist.twist.linear, header);
+     969           0 :             if (res) {
+     970           0 :               measurement_t measurement;
+     971           0 :               measurement = res.value();
+     972           0 :               return measurement;
+     973             :             } else {
+     974           0 :               return {};
+     975             :             }
+     976             :           } else {
+     977           0 :             measurement_t measurement;
+     978           0 :             measurement(0) = msg->twist.twist.linear.z;
+     979           0 :             return measurement;
+     980             :           }
+     981             :           break;
+     982             :         }
+     983             : 
+     984           0 :         default: {
+     985           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str());
+     986           0 :           return {};
+     987             :         }
+     988             :       }
+     989             :       break;
+     990             :     }
+     991             : 
+     992             :     // handle heading estimators
+     993           0 :     case EstimatorType_t::HEADING: {
+     994             : 
+     995           0 :       switch (state_id_) {
+     996             : 
+     997           0 :         case StateId_t::POSITION: {
+     998           0 :           measurement_t                               measurement;
+     999           0 :           std::unique_ptr<mrs_lib::AttitudeConverter> attitude;
+    1000             : 
+    1001           0 :           if (transform_to_frame_enabled_) {
+    1002             : 
+    1003           0 :             auto res = ch_->transformer->getTransform(transform_from_frame_, transform_to_frame_, msg->header.stamp);
+    1004           0 :             if (!res) {
+    1005           0 :               ROS_WARN_THROTTLE(1.0, "[%s]: could not find transform from '%s' to '%s'", ros::this_node::getName().c_str(), transform_from_frame_.c_str(),
+    1006             :                                 transform_to_frame_.c_str());
+    1007           0 :               return {};
+    1008             :             }
+    1009             : 
+    1010             :             // R_to_from = R^to_from
+    1011           0 :             Eigen::Matrix3d R_to_from = mrs_lib::AttitudeConverter(res.value().transform.rotation);
+    1012             : 
+    1013           0 :             Eigen::Matrix3d R_odom = mrs_lib::AttitudeConverter(msg->pose.pose.orientation);
+    1014             : 
+    1015             :             // obtain heading from orientation
+    1016           0 :             attitude = std::make_unique<mrs_lib::AttitudeConverter>(R_to_from * R_odom);
+    1017             : 
+    1018             :           } else {
+    1019           0 :             attitude = std::make_unique<mrs_lib::AttitudeConverter>(msg->pose.pose.orientation);
+    1020             :           }
+    1021             : 
+    1022             :           try {
+    1023             :             // obtain heading from orientation
+    1024           0 :             measurement(StateId_t::POSITION) = attitude->getHeading();
+    1025             : 
+    1026             :             // subtract initial heading to start with zero heading
+    1027           0 :             if (init_hdg_in_zero_ && got_first_hdg_measurement_) {
+    1028           0 :               measurement(StateId_t::POSITION) = measurement(StateId_t::POSITION) - first_hdg_measurement_;
+    1029             :             }
+    1030             : 
+    1031             :             // unwrap heading wrt previous measurement
+    1032           0 :             if (got_first_hdg_measurement_) {
+    1033           0 :               measurement(StateId_t::POSITION) =
+    1034           0 :                   mrs_lib::geometry::radians::unwrap(measurement(StateId_t::POSITION), prev_hdg_measurement_(StateId_t::POSITION));
+    1035             :             } else {
+    1036           0 :               first_hdg_measurement_     = measurement(StateId_t::POSITION);
+    1037           0 :               got_first_hdg_measurement_ = true;
+    1038             :             }
+    1039           0 :             prev_hdg_measurement_ = measurement;
+    1040           0 :             return measurement;
+    1041             :           }
+    1042           0 :           catch (...) {
+    1043           0 :             ROS_ERROR_THROTTLE(1.0, "[%s]: failed to obtain heading", getPrintName().c_str());
+    1044           0 :             return {};
+    1045             :           }
+    1046             :           break;
+    1047             :         }
+    1048             : 
+    1049             :           /* case StateId_t::VELOCITY: { */
+    1050             :           /*   try { */
+    1051             :           /*     measurement_t measurement; */
+    1052             :           /*     measurement(0) = mrs_lib::AttitudeConverter(msg->pose.pose.orientation).getHeadingRate(msg->twist.twist.angular); */
+    1053             :           /*     return measurement; */
+    1054             :           /*   } */
+    1055             :           /*   catch (...) { */
+    1056             :           /*     ROS_ERROR_THROTTLE(1.0, "[%s]: Exception caught during getting heading rate (getCorrectionFromOdometry())", getPrintName().c_str()); */
+    1057             :           /*     return {}; */
+    1058             :           /*   } */
+    1059             :           /*   break; */
+    1060             :           /* } */
+    1061             : 
+    1062           0 :         default: {
+    1063           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str());
+    1064           0 :           return {};
+    1065             :         }
+    1066             :       }
+    1067           0 :       break;
+    1068             :     }
+    1069             : 
+    1070             :     // handle latalt estimators
+    1071           0 :     case EstimatorType_t::LATALT: {
+    1072             : 
+    1073           0 :       switch (state_id_) {
+    1074             : 
+    1075           0 :         case StateId_t::POSITION: {
+    1076           0 :           measurement_t measurement;
+    1077           0 :           if (transform_to_frame_enabled_) {
+    1078           0 :             std_msgs::Header header = msg->header;
+    1079           0 :             header.frame_id         = transform_from_frame_;
+    1080           0 :             auto res                = getInFrame(msg->pose.pose.position, header, transform_to_frame_);
+    1081           0 :             if (res) {
+    1082           0 :               measurement_t measurement;
+    1083           0 :               measurement(0) = res.value().x;
+    1084           0 :               measurement(1) = res.value().y;
+    1085           0 :               measurement(2) = res.value().z;
+    1086           0 :               return measurement;
+    1087             :             } else {
+    1088           0 :               ROS_WARN_THROTTLE(1.0, "[%s]: could not transform vel from odom", ros::this_node::getName().c_str());
+    1089           0 :               return {};
+    1090             :             }
+    1091             : 
+    1092             :           } else {
+    1093           0 :             measurement(0) = msg->pose.pose.position.x;
+    1094           0 :             measurement(1) = msg->pose.pose.position.y;
+    1095           0 :             measurement(2) = msg->pose.pose.position.z;
+    1096             :           }
+    1097           0 :           return measurement;
+    1098             :           break;
+    1099             :         }
+    1100             : 
+    1101           0 :         case StateId_t::VELOCITY: {
+    1102           0 :           if (is_in_body_frame_) {
+    1103           0 :             std_msgs::Header header = msg->header;
+    1104           0 :             header.frame_id         = ch_->frames.ns_fcu;  // message in odometry is published in body frame
+    1105           0 :             auto res                = getVecInFrame(msg->twist.twist.linear, header, ns_frame_id_ + "_att_only");
+    1106           0 :             if (res) {
+    1107           0 :               measurement_t measurement;
+    1108           0 :               measurement = res.value();
+    1109           0 :               return measurement;
+    1110             :             } else {
+    1111           0 :               ROS_WARN_THROTTLE(1.0, "[%s]: could not transform vel from odom", ros::this_node::getName().c_str());
+    1112           0 :               return {};
+    1113             :             }
+    1114             :           } else {
+    1115           0 :             measurement_t measurement;
+    1116           0 :             measurement(0) = msg->twist.twist.linear.x;
+    1117           0 :             measurement(1) = msg->twist.twist.linear.y;
+    1118           0 :             measurement(2) = msg->twist.twist.linear.z;
+    1119           0 :             return measurement;
+    1120             :           }
+    1121             :           break;
+    1122             :         }
+    1123             : 
+    1124           0 :         default: {
+    1125           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str());
+    1126           0 :           return {};
+    1127             :         }
+    1128             :       }
+    1129             :       break;
+    1130             :     }
+    1131             :   }
+    1132             : 
+    1133           0 :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+    1134           0 :   return {};
+    1135             : }
+    1136             : /*//}*/
+    1137             : 
+    1138             : /*//{ callbackPoseStamped() */
+    1139             : template <int n_measurements>
+    1140           0 : void Correction<n_measurements>::callbackPoseStamped(const geometry_msgs::PoseStamped::ConstPtr msg) {
+    1141             : 
+    1142           0 :   if (!is_initialized_) {
+    1143           0 :     return;
+    1144             :   }
+    1145             : 
+    1146           0 :   auto res = getCorrectionFromPoseStamped(msg);
+    1147           0 :   if (res) {
+    1148           0 :     applyCorrection(res.value(), msg->header.stamp);
+    1149             :   }
+    1150             : }
+    1151             : /*//}*/
+    1152             : 
+    1153             : /*//{ getCorrectionFromPoseStamped() */
+    1154             : template <int n_measurements>
+    1155           0 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromPoseStamped(
+    1156             :     const geometry_msgs::PoseStampedConstPtr msg) {
+    1157             : 
+    1158           0 :   switch (est_type_) {
+    1159             : 
+    1160             :     // handle lateral estimators
+    1161           0 :     case EstimatorType_t::LATERAL: {
+    1162             : 
+    1163           0 :       switch (state_id_) {
+    1164             : 
+    1165           0 :         case StateId_t::POSITION: {
+    1166           0 :           measurement_t measurement;
+    1167           0 :           measurement(0) = msg->pose.position.x;
+    1168           0 :           measurement(1) = msg->pose.position.y;
+    1169           0 :           return measurement;
+    1170             :           break;
+    1171             :         }
+    1172             : 
+    1173           0 :         default: {
+    1174           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromPoseStamped() switch", getPrintName().c_str());
+    1175           0 :           return {};
+    1176             :         }
+    1177             :       }
+    1178             :       break;
+    1179             :     }
+    1180             : 
+    1181             :     // handle altitude estimators
+    1182           0 :     case EstimatorType_t::ALTITUDE: {
+    1183             : 
+    1184           0 :       switch (state_id_) {
+    1185             : 
+    1186           0 :         case StateId_t::POSITION: {
+    1187           0 :           measurement_t measurement;
+    1188           0 :           measurement(0) = msg->pose.position.z;
+    1189           0 :           return measurement;
+    1190             :           break;
+    1191             :         }
+    1192             : 
+    1193           0 :         default: {
+    1194           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromPoseStamped() switch", getPrintName().c_str());
+    1195           0 :           return {};
+    1196             :         }
+    1197             :       }
+    1198             :       break;
+    1199             :     }
+    1200             : 
+    1201             :     // handle heading estimators
+    1202           0 :     case EstimatorType_t::HEADING: {
+    1203             : 
+    1204           0 :       switch (state_id_) {
+    1205             : 
+    1206           0 :         case StateId_t::POSITION: {
+    1207           0 :           measurement_t measurement;
+    1208             :           try {
+    1209             :             // obtain heading from orientation
+    1210           0 :             measurement(StateId_t::POSITION) = mrs_lib::AttitudeConverter(msg->pose.orientation).getHeading();
+    1211             :             // unwrap heading wrt previous measurement
+    1212           0 :             if (got_first_hdg_measurement_) {
+    1213           0 :               measurement(StateId_t::POSITION) = mrs_lib::geometry::radians::unwrap(measurement(POSITION), prev_hdg_measurement_(POSITION));
+    1214             :             } else {
+    1215           0 :               got_first_hdg_measurement_ = true;
+    1216             :             }
+    1217           0 :             prev_hdg_measurement_ = measurement;
+    1218           0 :             return measurement;
+    1219             :           }
+    1220           0 :           catch (...) {
+    1221           0 :             ROS_ERROR_THROTTLE(1.0, "[%s]: failed to obtain heading", getPrintName().c_str());
+    1222           0 :             return {};
+    1223             :           }
+    1224             :           break;
+    1225             :         }
+    1226             : 
+    1227           0 :         default: {
+    1228           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromPoseStamped() switch", getPrintName().c_str());
+    1229           0 :           return {};
+    1230             :         }
+    1231             :       }
+    1232             :       break;
+    1233             :     }
+    1234             : 
+    1235             :     // handle latalt estimators
+    1236           0 :     case EstimatorType_t::LATALT: {
+    1237             : 
+    1238           0 :       switch (state_id_) {
+    1239             : 
+    1240           0 :         case StateId_t::POSITION: {
+    1241           0 :           measurement_t measurement;
+    1242           0 :           measurement(0) = msg->pose.position.x;
+    1243           0 :           measurement(1) = msg->pose.position.y;
+    1244           0 :           measurement(2) = msg->pose.position.z;
+    1245           0 :           return measurement;
+    1246             :           break;
+    1247             :         }
+    1248             : 
+    1249           0 :         default: {
+    1250           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromPoseStamped() switch", getPrintName().c_str());
+    1251           0 :           return {};
+    1252             :         }
+    1253             :       }
+    1254             :       break;
+    1255             :     }
+    1256             :   }
+    1257             : 
+    1258           0 :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+    1259           0 :   return {};
+    1260             : }
+    1261             : /*//}*/
+    1262             : 
+    1263             : /*//{ callbackRange() */
+    1264             : template <int n_measurements>
+    1265      265563 : void Correction<n_measurements>::callbackRange(const sensor_msgs::Range::ConstPtr msg) {
+    1266             : 
+    1267      265563 :   if (!is_initialized_) {
+    1268           0 :     return;
+    1269             :   }
+    1270             : 
+    1271      264608 :   auto res = getCorrectionFromRange(msg);
+    1272      265830 :   if (res) {
+    1273      265680 :     applyCorrection(res.value(), msg->header.stamp);
+    1274             :   }
+    1275             : }
+    1276             : /*//}*/
+    1277             : 
+    1278             : /*//{ getCorrectionFromRange() */
+    1279             : template <int n_measurements>
+    1280      268897 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromRange(const sensor_msgs::RangeConstPtr msg) {
+    1281             : 
+    1282      268897 :   if (!range_enabled_) {
+    1283           0 :     ROS_INFO_THROTTLE(1.0, "[%s]: fusing range corrections is disabled", getPrintName().c_str());
+    1284           0 :     return {};
+    1285             :   }
+    1286             : 
+    1287      268897 :   if (!std::isfinite(msg->range)) {
+    1288           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: received value: %f. Not using this correction.", getPrintName().c_str(), msg->range);
+    1289           0 :     return {};
+    1290             :   }
+    1291             : 
+    1292      267058 :   const double eps = 1e-3;
+    1293      267058 :   if (msg->range <= msg->min_range + eps || msg->range >= msg->max_range - eps) {
+    1294          11 :     ROS_WARN_THROTTLE(1.0, "[%s]: range measurement %.2f outside of its valid range (%.2f, %.2f)", ros::this_node::getName().c_str(), msg->range,
+    1295             :                       msg->min_range, msg->max_range);
+    1296          11 :     return {};
+    1297             :   }
+    1298             : 
+    1299      538023 :   geometry_msgs::PoseStamped range_point;
+    1300             : 
+    1301      267889 :   range_point.header           = msg->header;
+    1302      269694 :   range_point.pose.position.x  = msg->range;
+    1303      267545 :   range_point.pose.position.y  = 0;
+    1304      267545 :   range_point.pose.position.z  = 0;
+    1305      267545 :   range_point.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    1306             : 
+    1307      538901 :   auto res = ch_->transformer->transformSingle(range_point, ch_->frames.ns_fcu_untilted);
+    1308             : 
+    1309      270139 :   Correction::measurement_t measurement;
+    1310             : 
+    1311      270134 :   if (res) {
+    1312      269818 :     measurement(0) = -res.value().pose.position.z;
+    1313      269814 :     return measurement;
+    1314             :   } else {
+    1315         319 :     ROS_ERROR_THROTTLE(1.0, "[%s]: Could not transform range measurement to %s. Not using this correction.", getPrintName().c_str(),
+    1316             :                        ch_->frames.ns_fcu_untilted.c_str());
+    1317         319 :     return {};
+    1318             :   }
+    1319             : }
+    1320             : /*//}*/
+    1321             : 
+    1322             : /*//{ callbackImu() */
+    1323             : template <int n_measurements>
+    1324           0 : void Correction<n_measurements>::callbackImu(const sensor_msgs::Imu::ConstPtr msg) {
+    1325             : 
+    1326           0 :   if (!is_initialized_) {
+    1327           0 :     return;
+    1328             :   }
+    1329             : 
+    1330           0 :   auto res = getCorrectionFromImu(msg);
+    1331           0 :   if (res) {
+    1332           0 :     applyCorrection(res.value(), msg->header.stamp);
+    1333             :   } else {
+    1334           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: Could not obtain correction from Imu msg", getPrintName().c_str());
+    1335             :   }
+    1336             : }
+    1337             : /*//}*/
+    1338             : 
+    1339             : /*//{ getCorrectionFromImu() */
+    1340             : template <int n_measurements>
+    1341           0 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromImu(const sensor_msgs::ImuConstPtr msg) {
+    1342             : 
+    1343           0 :   switch (est_type_) {
+    1344             : 
+    1345             :     // handle lateral estimators
+    1346           0 :     case EstimatorType_t::LATERAL: {
+    1347             : 
+    1348           0 :       switch (state_id_) {
+    1349             : 
+    1350           0 :         case StateId_t::ACCELERATION: {
+    1351           0 :           if (is_in_body_frame_) {
+    1352           0 :             auto res = getVecInFrame(msg->linear_acceleration, msg->header, ns_frame_id_ + "_att_only");
+    1353           0 :             if (res) {
+    1354           0 :               measurement_t measurement;
+    1355           0 :               measurement = res.value();
+    1356           0 :               return measurement;
+    1357             :             } else {
+    1358           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());
+    1359           0 :               return {};
+    1360             :             }
+    1361             :           } else {
+    1362           0 :             measurement_t measurement;
+    1363           0 :             measurement(0) = msg->linear_acceleration.x;
+    1364           0 :             measurement(1) = msg->linear_acceleration.y;
+    1365           0 :             return measurement;
+    1366             :           }
+    1367             :           break;
+    1368             :         }
+    1369             : 
+    1370           0 :         default: {
+    1371           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromImu() switch", getPrintName().c_str());
+    1372           0 :           return {};
+    1373             :         }
+    1374             :       }
+    1375             :       break;
+    1376             :     }
+    1377             : 
+    1378             :     // handle altitude estimators
+    1379           0 :     case EstimatorType_t::ALTITUDE: {
+    1380             : 
+    1381           0 :       switch (state_id_) {
+    1382             : 
+    1383           0 :         case StateId_t::ACCELERATION: {
+    1384           0 :           if (is_in_body_frame_) {
+    1385           0 :             auto res = getZVelUntilted(msg->linear_acceleration, msg->header);
+    1386           0 :             if (res) {
+    1387           0 :               measurement_t measurement;
+    1388           0 :               measurement = res.value();
+    1389           0 :               measurement(0) -= gravity_norm_;
+    1390           0 :               return measurement;
+    1391             :             } else {
+    1392           0 :               ROS_WARN_THROTTLE(1.0, "[%s]: Could not obtain IMU Z acceleration", getPrintName().c_str());
+    1393           0 :               return {};
+    1394             :             }
+    1395             :           } else {
+    1396           0 :             measurement_t measurement;
+    1397           0 :             measurement(0) = msg->linear_acceleration.z - gravity_norm_;
+    1398           0 :             return measurement;
+    1399             :           }
+    1400             :           break;
+    1401             :         }
+    1402             : 
+    1403           0 :         default: {
+    1404           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromImu() switch", getPrintName().c_str());
+    1405           0 :           return {};
+    1406             :         }
+    1407             :       }
+    1408             :       break;
+    1409             :     }
+    1410             : 
+    1411             :     // handle heading estimators
+    1412           0 :     case EstimatorType_t::HEADING: {
+    1413             : 
+    1414           0 :       switch (state_id_) {
+    1415             : 
+    1416           0 :         case StateId_t::VELOCITY: {
+    1417           0 :           geometry_msgs::Quaternion orientation;
+    1418           0 :           auto                      res = ch_->transformer->getTransform(ch_->frames.ns_fcu_untilted, ch_->frames.ns_fcu, ros::Time::now());
+    1419           0 :           if (res) {
+    1420           0 :             orientation = res.value().transform.rotation;
+    1421             :           } else {
+    1422           0 :             ROS_ERROR_THROTTLE(1.0, "[%s]: Could not obtain transform from %s to %s. Not using this correction.", getPrintName().c_str(),
+    1423             :                                ch_->frames.ns_fcu_untilted.c_str(), ch_->frames.ns_fcu.c_str());
+    1424           0 :             return {};
+    1425             :           }
+    1426             : 
+    1427           0 :           measurement_t measurement;
+    1428           0 :           measurement(0) = mrs_lib::AttitudeConverter(orientation).getHeadingRate(msg->angular_velocity);
+    1429           0 :           return measurement;
+    1430             :           break;
+    1431             :         }
+    1432             : 
+    1433           0 :         default: {
+    1434           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromImu() switch", getPrintName().c_str());
+    1435           0 :           return {};
+    1436             :         }
+    1437             :       }
+    1438             :       break;
+    1439             :     }
+    1440             : 
+    1441             :     // handle latalt estimators
+    1442           0 :     case EstimatorType_t::LATALT: {
+    1443             : 
+    1444           0 :       switch (state_id_) {
+    1445             : 
+    1446           0 :         case StateId_t::ACCELERATION: {
+    1447           0 :           if (is_in_body_frame_) {
+    1448           0 :             auto res = getVecInFrame(msg->linear_acceleration, msg->header, ns_frame_id_ + "_att_only");
+    1449           0 :             if (res) {
+    1450           0 :               measurement_t measurement;
+    1451           0 :               measurement = res.value();
+    1452           0 :               return measurement;
+    1453             :             } else {
+    1454           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());
+    1455           0 :               return {};
+    1456             :             }
+    1457             :           } else {
+    1458           0 :             measurement_t measurement;
+    1459           0 :             measurement(0) = msg->linear_acceleration.x;
+    1460           0 :             measurement(1) = msg->linear_acceleration.y;
+    1461           0 :             measurement(2) = msg->linear_acceleration.z;
+    1462           0 :             return measurement;
+    1463             :           }
+    1464             :           break;
+    1465             :         }
+    1466             : 
+    1467           0 :         default: {
+    1468           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromImu() switch", getPrintName().c_str());
+    1469           0 :           return {};
+    1470             :         }
+    1471             :       }
+    1472             :       break;
+    1473             :     }
+    1474             :   }
+    1475           0 :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+    1476           0 :   return {};
+    1477             : }
+    1478             : /*//}*/
+    1479             : 
+    1480             : /*//{ callbackRtk() */
+    1481             : template <int n_measurements>
+    1482        5599 : void Correction<n_measurements>::callbackRtk(const mrs_msgs::RtkGps::ConstPtr msg) {
+    1483             : 
+    1484        5599 :   if (!is_initialized_) {
+    1485           0 :     return;
+    1486             :   }
+    1487             : 
+    1488        5597 :   auto res = getCorrectionFromRtk(msg);
+    1489        5606 :   if (res) {
+    1490        5599 :     applyCorrection(res.value(), msg->header.stamp);
+    1491             :   }
+    1492             : }
+    1493             : /*//}*/
+    1494             : 
+    1495             : /*//{ getCorrectionFromRtk() */
+    1496             : template <int n_measurements>
+    1497        5709 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromRtk(const mrs_msgs::RtkGpsConstPtr msg) {
+    1498             : 
+    1499       11419 :   geometry_msgs::PoseStamped rtk_pos;
+    1500             : 
+    1501        5696 :   if (!std::isfinite(msg->gps.latitude)) {
+    1502           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in RTK variable \"msg->latitude\"!!!", getPrintName().c_str());
+    1503           0 :     return {};
+    1504             :   }
+    1505             : 
+    1506        5700 :   if (!std::isfinite(msg->gps.longitude)) {
+    1507           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in RTK variable \"msg->longitude\"!!!", getPrintName().c_str());
+    1508           0 :     return {};
+    1509             :   }
+    1510             : 
+    1511        5690 :   if (!std::isfinite(msg->gps.altitude)) {
+    1512           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in RTK variable \"msg->altitude\"!!!", getPrintName().c_str());
+    1513           0 :     return {};
+    1514             :   }
+    1515             : 
+    1516        5689 :   if (msg->fix_type.fix_type != mrs_msgs::RtkFixType::RTK_FIX) {
+    1517           0 :     ROS_INFO_THROTTLE(1.0, "[%s] %s RTK FIX", getPrintName().c_str(), Support::waiting_for_string.c_str());
+    1518           0 :     return {};
+    1519             :   }
+    1520             : 
+    1521        5698 :   rtk_pos.header = msg->header;
+    1522        5706 :   mrs_lib::UTM(msg->gps.latitude, msg->gps.longitude, &rtk_pos.pose.position.x, &rtk_pos.pose.position.y);
+    1523        5672 :   rtk_pos.pose.position.z  = msg->gps.altitude;
+    1524        5673 :   rtk_pos.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    1525             : 
+    1526        5694 :   rtk_pos.pose.position.x -= ch_->world_origin.x;
+    1527        5698 :   rtk_pos.pose.position.y -= ch_->world_origin.y;
+    1528             : 
+    1529        5696 :   Correction::measurement_t measurement;
+    1530             : 
+    1531             :   // transform the RTK position from antenna to FCU
+    1532        5694 :   auto res = transformRtkToFcu(rtk_pos);
+    1533        5710 :   if (res) {
+    1534        5710 :     rtk_pos.pose = res.value();
+    1535             :   } else {
+    1536           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: transform to fcu failed", getPrintName().c_str());
+    1537           0 :     return {};
+    1538             :   }
+    1539             : 
+    1540        5710 :   switch (est_type_) {
+    1541             : 
+    1542             :     // handle lateral estimators
+    1543        3879 :     case EstimatorType_t::LATERAL: {
+    1544             : 
+    1545        3879 :       switch (state_id_) {
+    1546             : 
+    1547        3879 :         case StateId_t::POSITION: {
+    1548        3879 :           measurement(0) = rtk_pos.pose.position.x;
+    1549        3879 :           measurement(1) = rtk_pos.pose.position.y;
+    1550        3879 :           return measurement;
+    1551             :           break;
+    1552             :         }
+    1553             : 
+    1554           0 :         default: {
+    1555           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromRtk() switch", getPrintName().c_str());
+    1556           0 :           return {};
+    1557             :         }
+    1558             :       }
+    1559             :       break;
+    1560             :     }
+    1561             : 
+    1562             :     // handle altitude estimators
+    1563        1831 :     case EstimatorType_t::ALTITUDE: {
+    1564             : 
+    1565        1831 :       switch (state_id_) {
+    1566             : 
+    1567        1831 :         case StateId_t::POSITION: {
+    1568        1831 :           measurement(0) = rtk_pos.pose.position.z;
+    1569        1831 :           if (!got_avg_init_z_) {
+    1570          22 :             getAvgInitZ(measurement(0));
+    1571          22 :             return {};
+    1572             :           }
+    1573        1809 :           measurement(0) -= init_z_avg_;
+    1574        1809 :           return measurement;
+    1575             :           break;
+    1576             :         }
+    1577             : 
+    1578           0 :         default: {
+    1579           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromRtk() switch", getPrintName().c_str());
+    1580           0 :           return {};
+    1581             :         }
+    1582             :       }
+    1583             :       break;
+    1584             :     }
+    1585             : 
+    1586           0 :     case EstimatorType_t::HEADING: {
+    1587           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: should not be possible to get into this branch of getCorrectionFromRtk() switch", getPrintName().c_str());
+    1588           0 :       return {};
+    1589             :       break;
+    1590             :     }
+    1591             : 
+    1592             :     // handle latalt estimators
+    1593           0 :     case EstimatorType_t::LATALT: {
+    1594             : 
+    1595           0 :       switch (state_id_) {
+    1596             : 
+    1597           0 :         case StateId_t::POSITION: {
+    1598           0 :           measurement(0) = rtk_pos.pose.position.x;
+    1599           0 :           measurement(1) = rtk_pos.pose.position.y;
+    1600           0 :           measurement(2) = rtk_pos.pose.position.z;
+    1601           0 :           return measurement;
+    1602             :           break;
+    1603             :         }
+    1604             : 
+    1605           0 :         default: {
+    1606           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromRtk() switch", getPrintName().c_str());
+    1607           0 :           return {};
+    1608             :         }
+    1609             :       }
+    1610             :       break;
+    1611             :     }
+    1612             :   }
+    1613             : 
+    1614           0 :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+    1615           0 :   return {};
+    1616             : }
+    1617             : /*//}*/
+    1618             : 
+    1619             : /*//{ callbackNavSatFix() */
+    1620             : template <int n_measurements>
+    1621           0 : void Correction<n_measurements>::callbackNavSatFix(const sensor_msgs::NavSatFix::ConstPtr msg) {
+    1622             : 
+    1623           0 :   if (!is_initialized_) {
+    1624           0 :     return;
+    1625             :   }
+    1626             : 
+    1627           0 :   auto res = getCorrectionFromNavSatFix(msg);
+    1628           0 :   if (res) {
+    1629           0 :     applyCorrection(res.value(), msg->header.stamp);
+    1630             :   }
+    1631             : }
+    1632             : /*//}*/
+    1633             : 
+    1634             : /*//{ getCorrectionFromNavSatFix() */
+    1635             : template <int n_measurements>
+    1636           0 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromNavSatFix(
+    1637             :     const sensor_msgs::NavSatFixConstPtr msg) {
+    1638             : 
+    1639           0 :   geometry_msgs::PointStamped navsatfix_pos;
+    1640             : 
+    1641           0 :   if (!std::isfinite(msg->latitude)) {
+    1642           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in NavSatFix variable \"msg->latitude\"!!!", getPrintName().c_str());
+    1643           0 :     return {};
+    1644             :   }
+    1645             : 
+    1646           0 :   if (!std::isfinite(msg->longitude)) {
+    1647           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in NavSatFix variable \"msg->longitude\"!!!", getPrintName().c_str());
+    1648           0 :     return {};
+    1649             :   }
+    1650             : 
+    1651           0 :   if (!std::isfinite(msg->altitude)) {
+    1652           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in NavSatFix variable \"msg->altitude\"!!!", getPrintName().c_str());
+    1653           0 :     return {};
+    1654             :   }
+    1655             : 
+    1656           0 :   if (msg->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX) {
+    1657           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NavSatFix has no GNSS fix!!!", getPrintName().c_str());
+    1658           0 :     return {};
+    1659             :   }
+    1660             : 
+    1661           0 :   navsatfix_pos.header = msg->header;
+    1662           0 :   mrs_lib::UTM(msg->latitude, msg->longitude, &navsatfix_pos.point.x, &navsatfix_pos.point.y);
+    1663           0 :   navsatfix_pos.point.x -= ch_->world_origin.x;
+    1664           0 :   navsatfix_pos.point.y -= ch_->world_origin.y;
+    1665           0 :   navsatfix_pos.point.z = msg->altitude;
+    1666             : 
+    1667           0 :   Correction::measurement_t measurement;
+    1668             : 
+    1669             :   // TODO transform position from GNSS antenna to FCU
+    1670             : 
+    1671           0 :   switch (est_type_) {
+    1672             : 
+    1673             :     // handle lateral estimators
+    1674           0 :     case EstimatorType_t::LATERAL: {
+    1675             : 
+    1676           0 :       switch (state_id_) {
+    1677             : 
+    1678           0 :         case StateId_t::POSITION: {
+    1679           0 :           measurement(0) = navsatfix_pos.point.x;
+    1680           0 :           measurement(1) = navsatfix_pos.point.y;
+    1681           0 :           return measurement;
+    1682             :           break;
+    1683             :         }
+    1684             : 
+    1685           0 :         default: {
+    1686           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromNavSatFix() switch", getPrintName().c_str());
+    1687           0 :           return {};
+    1688             :         }
+    1689             :       }
+    1690             :       break;
+    1691             :     }
+    1692             : 
+    1693             :     // handle altitude estimators
+    1694           0 :     case EstimatorType_t::ALTITUDE: {
+    1695             : 
+    1696           0 :       switch (state_id_) {
+    1697             : 
+    1698           0 :         case StateId_t::POSITION: {
+    1699           0 :           measurement(0) = navsatfix_pos.point.z;
+    1700           0 :           if (!got_avg_init_z_) {
+    1701           0 :             getAvgInitZ(measurement(0));
+    1702           0 :             return {};
+    1703             :           }
+    1704           0 :           measurement(0) -= init_z_avg_;
+    1705           0 :           return measurement;
+    1706             :           break;
+    1707             :         }
+    1708             : 
+    1709           0 :         default: {
+    1710           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromNavSatFix() switch", getPrintName().c_str());
+    1711           0 :           return {};
+    1712             :         }
+    1713             :       }
+    1714             :       break;
+    1715             :     }
+    1716             : 
+    1717           0 :     case EstimatorType_t::HEADING: {
+    1718           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: should not be possible to get into this branch of getCorrectionFromNavSatFix() switch", getPrintName().c_str());
+    1719           0 :       return {};
+    1720             :       break;
+    1721             :     }
+    1722             : 
+    1723             :     // handle latalt estimators
+    1724           0 :     case EstimatorType_t::LATALT: {
+    1725             : 
+    1726           0 :       switch (state_id_) {
+    1727             : 
+    1728           0 :         case StateId_t::POSITION: {
+    1729           0 :           measurement(0) = navsatfix_pos.point.x;
+    1730           0 :           measurement(1) = navsatfix_pos.point.y;
+    1731           0 :           measurement(2) = navsatfix_pos.point.z;
+    1732           0 :           return measurement;
+    1733             :           break;
+    1734             :         }
+    1735             : 
+    1736           0 :         default: {
+    1737           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromNavSatFix() switch", getPrintName().c_str());
+    1738           0 :           return {};
+    1739             :         }
+    1740             :       }
+    1741             :       break;
+    1742             :     }
+    1743             :   }
+    1744             : 
+    1745           0 :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+    1746           0 :   return {};
+    1747             : }
+    1748             : /*//}*/
+    1749             : 
+    1750             : /*//{ callbackMagHeading() */
+    1751             : template <int n_measurements>
+    1752           0 : void Correction<n_measurements>::callbackMagHeading(const mrs_msgs::Float64Stamped::ConstPtr msg) {
+    1753             : 
+    1754           0 :   if (!is_initialized_) {
+    1755           0 :     return;
+    1756             :   }
+    1757             : 
+    1758           0 :   auto res = getCorrectionFromMagHeading(msg);
+    1759           0 :   if (res) {
+    1760           0 :     applyCorrection(res.value(), msg->header.stamp);
+    1761             :   } else {
+    1762           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: Could not obtain correction from Float64Stamped msg", getPrintName().c_str());
+    1763             :   }
+    1764             : }
+    1765             : /*//}*/
+    1766             : 
+    1767             : /*//{ getCorrectionFromMagHeading() */
+    1768             : template <int n_measurements>
+    1769           0 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromMagHeading(
+    1770             :     const mrs_msgs::Float64StampedConstPtr msg) {
+    1771             : 
+    1772           0 :   switch (est_type_) {
+    1773             : 
+    1774             :     // handle lateral estimators
+    1775           0 :     case EstimatorType_t::LATERAL: {
+    1776             : 
+    1777           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: EstimatorType_t::LATERAL in getCorrectionFromMagHeading() not implemented", getPrintName().c_str());
+    1778           0 :       return {};
+    1779             :       break;
+    1780             :     }
+    1781             : 
+    1782             :     // handle altitude estimators
+    1783           0 :     case EstimatorType_t::ALTITUDE: {
+    1784             : 
+    1785           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: EstimatorType_t::ALTITUDE in getCorrectionFromMagHeading() not implemented", getPrintName().c_str());
+    1786           0 :       return {};
+    1787             :       break;
+    1788             :     }
+    1789             : 
+    1790             :     // handle heading estimators
+    1791           0 :     case EstimatorType_t::HEADING: {
+    1792             : 
+    1793           0 :       measurement_t measurement;
+    1794             : 
+    1795           0 :       const double mag_hdg = msg->value / 180 * M_PI;
+    1796             : 
+    1797           0 :       if (!got_first_mag_hdg_) {
+    1798           0 :         mag_hdg_previous_  = mag_hdg;
+    1799           0 :         got_first_mag_hdg_ = true;
+    1800             :       }
+    1801             : 
+    1802           0 :       measurement(0) = -mrs_lib::geometry::radians::unwrap(mag_hdg, mag_hdg_previous_) + M_PI / 2;  // may be weirdness of px4 heading (NED vs ENU or something)
+    1803           0 :       mag_hdg_previous_ = mag_hdg;
+    1804           0 :       return measurement;
+    1805             :     }
+    1806             : 
+    1807             :     // handle latalt estimators
+    1808           0 :     case EstimatorType_t::LATALT: {
+    1809             : 
+    1810           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: EstimatorType_t::LATALT in getCorrectionFromMagHeading() not implemented", getPrintName().c_str());
+    1811           0 :       return {};
+    1812             :       break;
+    1813             :     }
+    1814             :   }
+    1815             : 
+    1816           0 :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+    1817           0 :   return {};
+    1818             : }
+    1819             : /*//}*/
+    1820             : 
+    1821             : /*//{ callbackMagField() */
+    1822             : template <int n_measurements>
+    1823           0 : void Correction<n_measurements>::callbackMagField(const sensor_msgs::MagneticField::ConstPtr msg) {
+    1824             : 
+    1825           0 :   if (!is_initialized_) {
+    1826           0 :     return;
+    1827             :   }
+    1828             : 
+    1829           0 :   auto res = getCorrectionFromMagField(msg);
+    1830           0 :   if (res) {
+    1831           0 :     applyCorrection(res.value(), msg->header.stamp);
+    1832             :   } else {
+    1833           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: Could not obtain correction from sensor_msgs::MagneticField msg", getPrintName().c_str());
+    1834             :   }
+    1835             : }
+    1836             : /*//}*/
+    1837             : 
+    1838             : /*//{ getCorrectionFromMagField() */
+    1839             : template <int n_measurements>
+    1840           0 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromMagField(
+    1841             :     const sensor_msgs::MagneticFieldConstPtr msg) {
+    1842             : 
+    1843           0 :   switch (est_type_) {
+    1844             : 
+    1845             :     // handle lateral estimators
+    1846           0 :     case EstimatorType_t::LATERAL: {
+    1847             : 
+    1848           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: EstimatorType_t::LATERAL in getCorrectionFromMagField() not implemented", getPrintName().c_str());
+    1849           0 :       return {};
+    1850             :       break;
+    1851             :     }
+    1852             : 
+    1853             :     // handle altitude estimators
+    1854           0 :     case EstimatorType_t::ALTITUDE: {
+    1855             : 
+    1856           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: EstimatorType_t::ALTITUDE in getCorrectionFromMagField() not implemented", getPrintName().c_str());
+    1857           0 :       return {};
+    1858             :       break;
+    1859             :     }
+    1860             : 
+    1861             :     // handle heading estimators
+    1862           0 :     case EstimatorType_t::HEADING: {
+    1863             : 
+    1864             :       /* Eigen::Matrix3d rot; */
+    1865             : 
+    1866             :       /* auto res = ch_->transformer->getTransform(ch_->frames.ns_fcu, ch_->frames.ns_fcu_untilted, msg->header.stamp); */
+    1867             :       /* if (res) { */
+    1868             :       /*   rot = Eigen::Matrix3d(mrs_lib::AttitudeConverter(res.value().transform.rotation)); */
+    1869             :       /* } else { */
+    1870             :       /*   ROS_ERROR_THROTTLE(1.0, "[%s]: Could not obtain transform from %s to %s. Not using this correction.", getPrintName().c_str(), */
+    1871             :       /*                      ch_->frames.ns_fcu_untilted.c_str(), ch_->frames.ns_fcu.c_str()); */
+    1872             :       /*   return {}; */
+    1873             :       /* } */
+    1874             : 
+    1875           0 :       geometry_msgs::Vector3 mag_vec;
+    1876           0 :       mag_vec.x = msg->magnetic_field.x;
+    1877           0 :       mag_vec.y = msg->magnetic_field.y;
+    1878           0 :       mag_vec.z = msg->magnetic_field.z;
+    1879             : 
+    1880           0 :       if (transform_to_frame_enabled_) {
+    1881           0 :         std_msgs::Header header = msg->header;
+    1882           0 :         header.frame_id         = transform_from_frame_;
+    1883           0 :         auto res                = transformVecToFrame(mag_vec, header, transform_to_frame_);
+    1884           0 :         if (res) {
+    1885           0 :           mag_vec.x = res.value().x;
+    1886           0 :           mag_vec.y = res.value().y;
+    1887           0 :           mag_vec.z = res.value().z;
+    1888             :         } else {
+    1889           0 :           ROS_WARN_THROTTLE(1.0, "[%s]: could not transform mag field vector", getPrintName().c_str());
+    1890             :         }
+    1891             :       }
+    1892             :       
+    1893           0 :       geometry_msgs::PointStamped mag_vec_msg;
+    1894           0 :       mag_vec_msg.header.stamp = msg->header.stamp;
+    1895           0 :       mag_vec_msg.header.frame_id = transform_to_frame_;
+    1896           0 :       mag_vec_msg.point.x = mag_vec.x;
+    1897           0 :       mag_vec_msg.point.y = mag_vec.y;
+    1898           0 :       mag_vec_msg.point.z = mag_vec.z;
+    1899           0 :       ph_mag_field_untilted_.publish(mag_vec_msg);
+    1900           0 :       const double mag_hdg = atan2(mag_vec.y, mag_vec.x);
+    1901             :       /* const Eigen::Vector3d mag_vec(mag_vec_pt.x, mag_vec_pt.y, mag_vec_pt.z); */
+    1902             :       /* const Eigen::Vector3d proj_mag_field = rot * mag_vec; */
+    1903             :       /* mrs_msgs::Float64Stamped hdg_stamped; */
+    1904             :       /* hdg_stamped.header = msg->header; */
+    1905             :       /* hdg_stamped.value = atan2(proj_mag_field.y(), proj_mag_field.x()); */
+    1906             :       /* const double mag_hdg = atan2(proj_mag_field.y(), proj_mag_field.x()); */
+    1907             : 
+    1908           0 :       measurement_t measurement;
+    1909             : 
+    1910             :       /* const double mag_hdg = msg->value / 180 * M_PI; */
+    1911             : 
+    1912           0 :       if (!got_first_mag_hdg_) {
+    1913           0 :         mag_hdg_previous_  = mag_hdg;
+    1914           0 :         got_first_mag_hdg_ = true;
+    1915             :       }
+    1916             : 
+    1917           0 :       measurement(0) = -mrs_lib::geometry::radians::unwrap(mag_hdg, mag_hdg_previous_);  // may be weirdness of px4 heading (NED vs ENU or something)
+    1918           0 :       mag_hdg_previous_ = mag_hdg;
+    1919           0 :       return measurement;
+    1920             :     }
+    1921             : 
+    1922             :     // handle latalt estimators
+    1923           0 :     case EstimatorType_t::LATALT: {
+    1924             : 
+    1925           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: EstimatorType_t::LATALT in getCorrectionFromMagField() not implemented", getPrintName().c_str());
+    1926           0 :       return {};
+    1927             :       break;
+    1928             :     }
+    1929             :   }
+    1930             : 
+    1931           0 :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+    1932           0 :   return {};
+    1933             : }
+    1934             : /*//}*/
+    1935             : 
+    1936             : /*//{ callbackPoint() */
+    1937             : template <int n_measurements>
+    1938      218493 : void Correction<n_measurements>::callbackPoint(const geometry_msgs::PointStamped::ConstPtr msg) {
+    1939             : 
+    1940      218493 :   if (!is_initialized_) {
+    1941           0 :     return;
+    1942             :   }
+    1943             : 
+    1944      218419 :   auto res = getCorrectionFromPoint(msg);
+    1945      218224 :   if (res) {
+    1946      218323 :     applyCorrection(res.value(), msg->header.stamp);
+    1947             :   }
+    1948             : }
+    1949             : /*//}*/
+    1950             : 
+    1951             : /*//{ getCorrectionFromPoint() */
+    1952             : template <int n_measurements>
+    1953      222580 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromPoint(
+    1954             :     const geometry_msgs::PointStampedConstPtr msg) {
+    1955             : 
+    1956      222580 :   switch (est_type_) {
+    1957             : 
+    1958             :     // handle lateral estimators
+    1959      222580 :     case EstimatorType_t::LATERAL: {
+    1960             : 
+    1961      222580 :       switch (state_id_) {
+    1962             : 
+    1963      222470 :         case StateId_t::POSITION: {
+    1964      222470 :           measurement_t measurement;
+    1965      222474 :           measurement(0) = msg->point.x;
+    1966      222260 :           measurement(1) = msg->point.y;
+    1967      222375 :           return measurement;
+    1968             :           break;
+    1969             :         }
+    1970             : 
+    1971         110 :         default: {
+    1972         110 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromPoint() switch", getPrintName().c_str());
+    1973           0 :           return {};
+    1974             :         }
+    1975             :       }
+    1976             :       break;
+    1977             :     }
+    1978             : 
+    1979             :     // handle altitude estimators
+    1980           0 :     case EstimatorType_t::ALTITUDE: {
+    1981             : 
+    1982           0 :       switch (state_id_) {
+    1983             : 
+    1984           0 :         case StateId_t::POSITION: {
+    1985           0 :           measurement_t measurement;
+    1986           0 :           measurement(0) = msg->point.z;
+    1987           0 :           return measurement;
+    1988             :           break;
+    1989             :         }
+    1990             : 
+    1991           0 :         default: {
+    1992           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromPoint() switch", getPrintName().c_str());
+    1993           0 :           return {};
+    1994             :         }
+    1995             :       }
+    1996             :       break;
+    1997             :     }
+    1998             : 
+    1999             :     // handle heading estimators
+    2000           0 :     case EstimatorType_t::HEADING: {
+    2001             : 
+    2002           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: EstimatorType_t::Heading in getCorrectionFromPoint() not implemented", getPrintName().c_str());
+    2003           0 :       return {};
+    2004             :       break;
+    2005             :     }
+    2006             : 
+    2007             :     // handle latalt estimators
+    2008           0 :     case EstimatorType_t::LATALT: {
+    2009             : 
+    2010           0 :       switch (state_id_) {
+    2011             : 
+    2012           0 :         case StateId_t::POSITION: {
+    2013           0 :           measurement_t measurement;
+    2014           0 :           measurement(0) = msg->point.x;
+    2015           0 :           measurement(1) = msg->point.y;
+    2016           0 :           measurement(2) = msg->point.z;
+    2017           0 :           return measurement;
+    2018             :           break;
+    2019             :         }
+    2020             : 
+    2021           0 :         default: {
+    2022           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromPoint() switch", getPrintName().c_str());
+    2023           0 :           return {};
+    2024             :         }
+    2025             :       }
+    2026             :       break;
+    2027             :     }
+    2028             : 
+    2029           0 :     default: {
+    2030           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromPoint() switch", getPrintName().c_str());
+    2031           0 :       return {};
+    2032             :     }
+    2033             :   }
+    2034             : 
+    2035             : 
+    2036             :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+    2037             :   return {};
+    2038             : }
+    2039             : /*//}*/
+    2040             : 
+    2041             : /*//{ callbackVector() */
+    2042             : template <int n_measurements>
+    2043      239727 : void Correction<n_measurements>::callbackVector(const geometry_msgs::Vector3Stamped::ConstPtr msg) {
+    2044             : 
+    2045      239727 :   if (!is_initialized_) {
+    2046           0 :     return;
+    2047             :   }
+    2048             : 
+    2049      239527 :   auto res = getCorrectionFromVector(msg);
+    2050      239781 :   if (res) {
+    2051      239750 :     applyCorrection(res.value(), msg->header.stamp);
+    2052             :   } else {
+    2053          34 :     ROS_WARN_THROTTLE(1.0, "[%s]: Could not obtain correction from Vector3Stamped msg", getPrintName().c_str());
+    2054             :   }
+    2055             : }
+    2056             : /*//}*/
+    2057             : 
+    2058             : /*//{ getCorrectionFromVector() */
+    2059             : template <int n_measurements>
+    2060      239849 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromVector(
+    2061             :     const geometry_msgs::Vector3StampedConstPtr msg) {
+    2062             : 
+    2063      239849 :   switch (est_type_) {
+    2064             : 
+    2065             :     // handle lateral estimators
+    2066       11051 :     case EstimatorType_t::LATERAL: {
+    2067             : 
+    2068       11051 :       switch (state_id_) {
+    2069             : 
+    2070       11049 :         case StateId_t::VELOCITY: {
+    2071       11049 :           auto res = getVecInFrame(msg->vector, msg->header, ns_frame_id_ + "_att_only");
+    2072       11054 :           if (res) {
+    2073       11003 :             measurement_t measurement;
+    2074       11003 :             measurement = res.value();
+    2075       11004 :             return measurement;
+    2076             :           } else {
+    2077          50 :             return {};
+    2078             :           }
+    2079             :           break;
+    2080             :         }
+    2081             : 
+    2082           2 :         default: {
+    2083           2 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromVector() switch", getPrintName().c_str());
+    2084           0 :           return {};
+    2085             :         }
+    2086             :       }
+    2087             :       break;
+    2088             :     }
+    2089             : 
+    2090             :     // handle altitude estimators
+    2091      228787 :     case EstimatorType_t::ALTITUDE: {
+    2092             : 
+    2093      228787 :       switch (state_id_) {
+    2094             : 
+    2095      228794 :         case StateId_t::VELOCITY: {
+    2096      228794 :           auto res = getZVelUntilted(msg->vector, msg->header);
+    2097      228875 :           if (res) {
+    2098      228867 :             measurement_t measurement;
+    2099      228864 :             measurement = res.value();
+    2100      228862 :             return measurement;
+    2101             :           } else {
+    2102           9 :             ROS_WARN_THROTTLE(1.0, "[%s]: Could not obtain untilted Z velocity", getPrintName().c_str());
+    2103           9 :             return {};
+    2104             :           }
+    2105             :           break;
+    2106             :         }
+    2107             : 
+    2108           9 :         default: {
+    2109           9 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromVector() switch", getPrintName().c_str());
+    2110           0 :           return {};
+    2111             :         }
+    2112             :       }
+    2113             :       break;
+    2114             :     }
+    2115             : 
+    2116             :     // handle heading estimators
+    2117           0 :     case EstimatorType_t::HEADING: {
+    2118             : 
+    2119           0 :       switch (state_id_) {
+    2120             : 
+    2121           0 :         case StateId_t::VELOCITY: {
+    2122             :           try {
+    2123           0 :             if (!sh_orientation_.hasMsg()) {
+    2124           0 :               ROS_INFO_THROTTLE(1.0, "[%s]: %s orientation on topic: %s", getPrintName().c_str(), Support::waiting_for_string.c_str(),
+    2125             :                                 orientation_topic_.c_str());
+    2126           0 :               return {};
+    2127             :             }
+    2128           0 :             measurement_t measurement;
+    2129           0 :             measurement(0) = mrs_lib::AttitudeConverter(sh_orientation_.getMsg()->quaternion).getHeadingRate(msg->vector);
+    2130           0 :             return measurement;
+    2131             :           }
+    2132           0 :           catch (...) {
+    2133           0 :             ROS_ERROR_THROTTLE(1.0, "[%s]: Exception caught during getting heading rate (getCorrectionFromVector())", getPrintName().c_str());
+    2134           0 :             return {};
+    2135             :           }
+    2136             :           break;
+    2137             :         }
+    2138             : 
+    2139           0 :         default: {
+    2140           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromVector() switch", getPrintName().c_str());
+    2141           0 :           return {};
+    2142             :         }
+    2143             :       }
+    2144             :       break;
+    2145             :     }
+    2146             : 
+    2147             :     // handle lateral estimators
+    2148           0 :     case EstimatorType_t::LATALT: {
+    2149             : 
+    2150           0 :       switch (state_id_) {
+    2151             : 
+    2152           0 :         case StateId_t::VELOCITY: {
+    2153           0 :           auto res = getVecInFrame(msg->vector, msg->header, ns_frame_id_ + "_att_only");
+    2154           0 :           if (res) {
+    2155           0 :             measurement_t measurement;
+    2156           0 :             measurement = res.value();
+    2157           0 :             return measurement;
+    2158             :           } else {
+    2159           0 :             return {};
+    2160             :           }
+    2161             :           break;
+    2162             :         }
+    2163             : 
+    2164           0 :         default: {
+    2165           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromVector() switch", getPrintName().c_str());
+    2166           0 :           return {};
+    2167             :         }
+    2168             :       }
+    2169             :       break;
+    2170             :     }
+    2171             :   }
+    2172             : 
+    2173          22 :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+    2174           0 :   return {};
+    2175             : }
+    2176             : /*//}*/
+    2177             : 
+    2178             : /*//{ getCorrectionFromQuat() */
+    2179             : template <int n_measurements>
+    2180           0 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getCorrectionFromQuat([
+    2181             :     [maybe_unused]] const geometry_msgs::QuaternionStampedConstPtr msg) {
+    2182             : 
+    2183           0 :   switch (est_type_) {
+    2184             : 
+    2185             :       // handle lateral estimators
+    2186             :       /* case EstimatorType_t::LATERAL: { */
+    2187             : 
+    2188             :       /*   default: { */
+    2189             :       /*     ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str()); */
+    2190             :       /*     return {}; */
+    2191             :       /*   } break; */
+    2192             :       /* } */
+    2193             : 
+    2194             :       /* // handle altitude estimators */
+    2195             :       /* case EstimatorType_t::ALTITUDE: { */
+    2196             : 
+    2197             :       /* default: { */
+    2198             :       /*     ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str()); */
+    2199             :       /*     return {}; */
+    2200             :       /*   } break; */
+    2201             :       /* } */
+    2202             : 
+    2203             :       /* // handle heading estimators */
+    2204             :       /* case EstimatorType_t::HEADING: { */
+    2205             : 
+    2206             :       /*   switch (state_id_) { */
+    2207             : 
+    2208             :       /*     /1* default: { *1/ */
+    2209             :       /*       ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str()); */
+    2210             :       /*       return {}; */
+    2211             :       /*     } */
+    2212             :       /*   } */
+    2213             :     default: {
+    2214           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str());
+    2215           0 :       return {};
+    2216             :     }
+    2217             :   }
+    2218             : 
+    2219             :   ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
+    2220             :   return {};
+    2221             : }
+    2222             : /*//}*/
+    2223             : 
+    2224             : /*//{ applyCorrection() */
+    2225             : template <int n_measurements>
+    2226      729478 : void Correction<n_measurements>::applyCorrection(const measurement_t& meas, const ros::Time& stamp) {
+    2227             : 
+    2228             :   {
+    2229      729478 :     std::scoped_lock lock(mtx_msg_time_);
+    2230      729313 :     if (first_timestamp_) {
+    2231         399 :       prev_msg_time_   = stamp - ros::Duration(0.01);
+    2232         401 :       msg_time_        = stamp;
+    2233         401 :       healthy_time_    = ros::Time(0);
+    2234         399 :       first_timestamp_ = false;
+    2235             :     }
+    2236             : 
+    2237      729228 :     prev_msg_time_ = msg_time_;
+    2238      729228 :     msg_time_      = stamp;
+    2239      729228 :     healthy_time_ += msg_time_ - prev_msg_time_;
+    2240             :   }
+    2241             : 
+    2242      729216 :   MeasurementStamped meas_stamped;
+    2243      729200 :   meas_stamped.value = meas;
+    2244      729374 :   meas_stamped.stamp = stamp;
+    2245      729374 :   publishCorrection(meas_stamped, ph_correction_raw_);
+    2246      729574 :   if (process(meas_stamped.value)) {
+    2247      721144 :     publishCorrection(meas_stamped, ph_correction_proc_);
+    2248      721271 :     fun_apply_correction_(meas_stamped, getR(), getStateId());
+    2249             :   }
+    2250      729020 : }
+    2251             : /*//}*/
+    2252             : 
+    2253             : /* //{ callbackToggleRange() */
+    2254             : template <int n_measurements>
+    2255           0 : bool Correction<n_measurements>::callbackToggleRange(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    2256             : 
+    2257           0 :   if (!is_initialized_) {
+    2258           0 :     return false;
+    2259             :   }
+    2260             : 
+    2261           0 :   if (!range_enabled_ && req.data) {
+    2262           0 :     processors_["saturate"]->toggle(true);
+    2263             :   }
+    2264             : 
+    2265           0 :   range_enabled_ = req.data;
+    2266             : 
+    2267             :   // after enabling range we want to start correcting the altitude slowly
+    2268             : 
+    2269           0 :   res.success = true;
+    2270           0 :   res.message = (range_enabled_ ? "Range enabled" : "Range disabled");
+    2271             : 
+    2272           0 :   if (range_enabled_) {
+    2273             : 
+    2274           0 :     ROS_INFO("[%s]: Range enabled.", getPrintName().c_str());
+    2275             : 
+    2276             :   } else {
+    2277             : 
+    2278           0 :     ROS_INFO("[%s]: Range disabled", getPrintName().c_str());
+    2279             :   }
+    2280             : 
+    2281           0 :   return true;
+    2282             : }
+    2283             : 
+    2284             : //}
+    2285             : 
+    2286             : /*//{ getZVelUntilted() */
+    2287             : template <int n_measurements>
+    2288      228837 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getZVelUntilted(const geometry_msgs::Vector3& msg,
+    2289             :                                                                                                               const std_msgs::Header&       header) {
+    2290             : 
+    2291             :   // untilt the desired vector
+    2292      457710 :   geometry_msgs::PointStamped vel;
+    2293      228657 :   vel.point.x = msg.x;
+    2294      228657 :   vel.point.y = msg.y;
+    2295      228657 :   vel.point.z = msg.z;
+    2296      228657 :   vel.header  = header;
+    2297             :   /* vel.header.frame_id = ch_->frames.ns_fcu; */
+    2298      228777 :   vel.header.stamp = header.stamp;
+    2299             : 
+    2300      457652 :   auto res = ch_->transformer->transformSingle(vel, ch_->frames.ns_fcu_untilted);
+    2301      228876 :   if (res) {
+    2302      228867 :     measurement_t measurement;
+    2303      228867 :     measurement(0) = res.value().point.z;
+    2304      228865 :     return measurement;
+    2305             :   } else {
+    2306           9 :     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());
+    2307           9 :     return {};
+    2308             :   }
+    2309             : }
+    2310             : /*//}*/
+    2311             : 
+    2312             : /*//{ transformVecToFrame() */
+    2313             : template <int n_measurements>
+    2314       11054 : std::optional<geometry_msgs::Vector3> Correction<n_measurements>::transformVecToFrame(const geometry_msgs::Vector3& vec_in,
+    2315             :                                                                                       const std_msgs::Header& source_header, const std::string target_frame) {
+    2316             : 
+    2317       22108 :   geometry_msgs::Vector3Stamped vec;
+    2318       11050 :   vec.header = source_header;
+    2319       11049 :   vec.vector = vec_in;
+    2320             : 
+    2321       22103 :   auto res = ch_->transformer->transformSingle(vec, target_frame);
+    2322       11054 :   if (res) {
+    2323       11004 :     return res.value().vector;
+    2324             :   } else {
+    2325          50 :     ROS_WARN_THROTTLE(1.0, "[%s]: Transform of vector from %s to %s failed.", getPrintName().c_str(), vec.header.frame_id.c_str(), target_frame.c_str());
+    2326          50 :     return {};
+    2327             :   }
+    2328             : }
+    2329             : 
+    2330             : template <int n_measurements>
+    2331       11054 : std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_measurements>::getVecInFrame(const geometry_msgs::Vector3& vec_in,
+    2332             :                                                                                                             const std_msgs::Header&       source_header,
+    2333             :                                                                                                             const std::string             target_frame) {
+    2334             : 
+    2335       11054 :   measurement_t measurement;
+    2336             : 
+    2337       11048 :   auto res = transformVecToFrame(vec_in, source_header, target_frame);
+    2338       11054 :   if (res) {
+    2339       11004 :     measurement(0) = res.value().x;
+    2340       11004 :     measurement(1) = res.value().y;
+    2341             :     if (n_measurements == 3) {
+    2342             :       measurement(2) = res.value().z;
+    2343             :     }
+    2344       11004 :     return measurement;
+    2345             :   } else {
+    2346          50 :     return {};
+    2347             :   }
+    2348             : }
+    2349             : /*//}*/
+    2350             : 
+    2351             : /*//{ getInFrame() */
+    2352             : template <int n_measurements>
+    2353           0 : std::optional<geometry_msgs::Point> Correction<n_measurements>::getInFrame(const geometry_msgs::Point& pt_in, const std_msgs::Header& source_header,
+    2354             :                                                                            const std::string target_frame) {
+    2355             : 
+    2356           0 :   geometry_msgs::PointStamped pt;
+    2357           0 :   pt.header = source_header;
+    2358           0 :   pt.point  = pt_in;
+    2359             : 
+    2360           0 :   geometry_msgs::PointStamped transformed_pt;
+    2361           0 :   auto                        res = ch_->transformer->transformSingle(pt, target_frame);
+    2362           0 :   if (res) {
+    2363           0 :     transformed_pt = res.value();
+    2364           0 :     geometry_msgs::Point pt_out;
+    2365           0 :     pt_out = transformed_pt.point;
+    2366           0 :     return pt_out;
+    2367             :   } else {
+    2368           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: Transform of point from %s to %s failed.", getPrintName().c_str(), pt.header.frame_id.c_str(), target_frame.c_str());
+    2369           0 :     return {};
+    2370             :   }
+    2371             : }
+    2372             : /*//}*/
+    2373             : 
+    2374             : /*//{ transformRtkToFcu() */
+    2375             : template <int n_measurements>
+    2376        5706 : std::optional<geometry_msgs::Pose> Correction<n_measurements>::transformRtkToFcu(const geometry_msgs::PoseStamped& pose_in) const {
+    2377             : 
+    2378       11416 :   geometry_msgs::PoseStamped pose_tmp = pose_in;
+    2379             : 
+    2380             :   // inject current orientation into rtk pose
+    2381       11414 :   auto res1 = ch_->transformer->getTransform(ch_->frames.ns_fcu_untilted, ch_->frames.ns_fcu, ros::Time::now());
+    2382        5710 :   if (res1) {
+    2383        5710 :     pose_tmp.pose.orientation = res1.value().transform.rotation;
+    2384             :   } else {
+    2385           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: Could not obtain transform from %s to %s. Not using this correction.", getPrintName().c_str(),
+    2386             :                        ch_->frames.ns_fcu_untilted.c_str(), ch_->frames.ns_fcu.c_str());
+    2387           0 :     return {};
+    2388             :   }
+    2389             : 
+    2390             :   // invert tf
+    2391        5710 :   tf2::Transform             tf_utm_to_antenna = Support::tf2FromPose(pose_tmp.pose);
+    2392       11420 :   geometry_msgs::PoseStamped utm_in_antenna;
+    2393        5710 :   utm_in_antenna.pose            = Support::poseFromTf2(tf_utm_to_antenna.inverse());
+    2394        5710 :   utm_in_antenna.header.stamp    = pose_in.header.stamp;
+    2395        5710 :   utm_in_antenna.header.frame_id = ch_->frames.ns_rtk_antenna;
+    2396             : 
+    2397             :   // transform to fcu
+    2398       11420 :   geometry_msgs::PoseStamped utm_in_fcu;
+    2399        5710 :   utm_in_fcu.header.frame_id = ch_->frames.ns_fcu;
+    2400        5710 :   utm_in_fcu.header.stamp    = pose_in.header.stamp;
+    2401       11420 :   auto res2                  = ch_->transformer->transformSingle(utm_in_antenna, ch_->frames.ns_fcu);
+    2402             : 
+    2403        5710 :   if (res2) {
+    2404        5710 :     utm_in_fcu = res2.value();
+    2405             :   } else {
+    2406           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());
+    2407           0 :     return {};
+    2408             :   }
+    2409             : 
+    2410             :   // invert tf
+    2411        5710 :   tf2::Transform      tf_fcu_to_utm = Support::tf2FromPose(utm_in_fcu.pose);
+    2412        5710 :   geometry_msgs::Pose fcu_in_utm    = Support::poseFromTf2(tf_fcu_to_utm.inverse());
+    2413             : 
+    2414        5710 :   return fcu_in_utm;
+    2415             : }
+    2416             : /*//}*/
+    2417             : 
+    2418             : /*//{ getAvgInitZ() */
+    2419             : template <int n_measurements>
+    2420          22 : void Correction<n_measurements>::getAvgInitZ(const double z) {
+    2421             : 
+    2422          22 :   if (!got_avg_init_z_) {
+    2423             : 
+    2424          22 :     double z_avg = init_z_avg_ / got_z_counter_;
+    2425             : 
+    2426          22 :     if (got_z_counter_ < 10 || (got_z_counter_ < 300 && std::fabs(z - z_avg) > 0.1)) {
+    2427             : 
+    2428          20 :       init_z_avg_ += z;
+    2429          20 :       got_z_counter_++;
+    2430          20 :       z_avg = init_z_avg_ / got_z_counter_;
+    2431          20 :       ROS_INFO("[%s]: AMSL altitude sample #%d: %.2f; avg: %.2f", getPrintName().c_str(), got_z_counter_, z, z_avg);
+    2432          20 :       return;
+    2433             : 
+    2434             :     } else {
+    2435             : 
+    2436           2 :       init_z_avg_     = z_avg;
+    2437           2 :       got_avg_init_z_ = true;
+    2438           2 :       ROS_INFO("[%s]: AMSL altitude avg: %f", getPrintName().c_str(), z_avg);
+    2439             :     }
+    2440             :   }
+    2441             : }
+    2442             : /*//}*/
+    2443             : 
+    2444             : /*//{ checkMsgDelay() */
+    2445             : template <int n_measurements>
+    2446             : void Correction<n_measurements>::checkMsgDelay(const ros::Time& msg_time) {
+    2447             : 
+    2448             :   const double delay = (ros::Time::now() - msg_time).toSec();
+    2449             :   if (delay > msg_delay_warn_limit_) {
+    2450             :     if (delay > msg_delay_limit_) {
+    2451             :       ROS_ERROR_THROTTLE(1.0, "[%s]: message too delayed (%.4f s)", getPrintName().c_str(), delay);
+    2452             :       is_delay_ok_ = false;
+    2453             :     } else {
+    2454             :       ROS_WARN_THROTTLE(5.0, "[%s]: message delayed (%.4f s)", getPrintName().c_str(), delay);
+    2455             :       is_delay_ok_ = true;
+    2456             :     }
+    2457             :   } else {
+    2458             :     is_delay_ok_ = true;
+    2459             :   }
+    2460             :   publishDelay(delay);
+    2461             : }
+    2462             : /*//}*/
+    2463             : 
+    2464             : /*//{ isTimestampOk() */
+    2465             : template <int n_measurements>
+    2466             : bool Correction<n_measurements>::isTimestampOk() {
+    2467             : 
+    2468             :   if (first_timestamp_) {
+    2469             :     return true;
+    2470             :   }
+    2471             : 
+    2472             :   ros::Time msg_time, prev_msg_time;
+    2473             :   {
+    2474             :     std::scoped_lock lock(mtx_msg_time_);
+    2475             :     msg_time      = msg_time_;
+    2476             :     prev_msg_time = prev_msg_time_;
+    2477             :   }
+    2478             :   const double delta = msg_time.toSec() - prev_msg_time.toSec();
+    2479             : 
+    2480             :   if (msg_time.toSec() <= 0.0) {
+    2481             :     ROS_ERROR_THROTTLE(1.0, "[%s]: current timestamp non-positive: %f", getPrintName().c_str(), msg_time.toSec());
+    2482             :     return false;
+    2483             :   }
+    2484             : 
+    2485             :   if (delta <= 0.0) {
+    2486             :     ROS_WARN_THROTTLE(1.0, "[%s]: time delta non-positive: %f", getPrintName().c_str(), delta);
+    2487             :     return true;
+    2488             :   }
+    2489             : 
+    2490             :   if (delta < 0.001) {
+    2491             :     ROS_WARN_THROTTLE(1.0, "[%s]: time delta too small: %f", getPrintName().c_str(), delta);
+    2492             :     return true;
+    2493             :   }
+    2494             : 
+    2495             :   if (delta > time_since_last_msg_limit_) {
+    2496             :     ROS_ERROR_THROTTLE(1.0, "[%s]: time since last msg too long %f > %f", getPrintName().c_str(), delta, time_since_last_msg_limit_);
+    2497             :     return false;
+    2498             :   }
+    2499             : 
+    2500             :   return true;
+    2501             : }  // namespace mrs_uav_state_estimators
+    2502             : /*//}*/
+    2503             : 
+    2504             : /*//{ isMsgComing() */
+    2505             : template <int n_measurements>
+    2506      709490 : bool Correction<n_measurements>::isMsgComing() {
+    2507             : 
+    2508      709490 :   if (first_timestamp_) {
+    2509           0 :     return true;
+    2510             :   }
+    2511             : 
+    2512      709460 :   const ros::Time msg_time = mrs_lib::get_mutexed(mtx_msg_time_, msg_time_);
+    2513      709497 :   const double    delta    = ros::Time::now().toSec() - msg_time.toSec();
+    2514             : 
+    2515      709516 :   if (msg_time.toSec() <= 0.0) {
+    2516           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: current timestamp non-positive: %f", getPrintName().c_str(), msg_time.toSec());
+    2517           0 :     return false;
+    2518             :   }
+    2519             : 
+    2520      709529 :   if (delta > time_since_last_msg_limit_) {
+    2521           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: time since last msg too long %f > %f", getPrintName().c_str(), delta, time_since_last_msg_limit_);
+    2522           0 :     return false;
+    2523             :   }
+    2524             : 
+    2525      709529 :   return true;
+    2526             : }  // namespace mrs_uav_state_estimators
+    2527             : /*//}*/
+    2528             : 
+    2529             : /*//{ createProcessorFromName() */
+    2530             : template <int n_measurements>
+    2531         370 : std::shared_ptr<Processor<n_measurements>> Correction<n_measurements>::createProcessorFromName(const std::string& name, ros::NodeHandle& nh) {
+    2532             : 
+    2533         370 :   if (name == "median_filter") {
+    2534          84 :     return std::make_shared<ProcMedianFilter<n_measurements>>(nh, getNamespacedName(), name, ch_, ph_);
+    2535         286 :   } else if (name == "saturate") {
+    2536          90 :     return std::make_shared<ProcSaturate<n_measurements>>(nh, getNamespacedName(), name, ch_, ph_, state_id_, fun_get_state_);
+    2537         196 :   } else if (name == "excessive_tilt") {
+    2538          84 :     return std::make_shared<ProcExcessiveTilt<n_measurements>>(nh, getNamespacedName(), name, ch_, ph_);
+    2539         112 :   } else if (name == "tf_to_world") {
+    2540         112 :     return std::make_shared<ProcTfToWorld<n_measurements>>(nh, getNamespacedName(), name, ch_, ph_);
+    2541           0 :   } else if (name == "mag_declination") {
+    2542           0 :     return std::make_shared<ProcMagDeclination<n_measurements>>(nh, getNamespacedName(), name, ch_, ph_);
+    2543             :   } else {
+    2544           0 :     ROS_ERROR("[%s]: requested invalid processor %s", getPrintName().c_str(), name.c_str());
+    2545           0 :     ros::shutdown();
+    2546             :   }
+    2547           0 :   return std::shared_ptr<Processor<n_measurements>>(nullptr);
+    2548             : }
+    2549             : /*//}*/
+    2550             : 
+    2551             : /*//{ process() */
+    2552             : template <int n_measurements>
+    2553      737979 : bool Correction<n_measurements>::process(Correction<n_measurements>::measurement_t& measurement) {
+    2554             : 
+    2555      737979 :   bool ok_flag   = true;
+    2556      737979 :   bool fuse_flag = true;
+    2557             : 
+    2558     1386371 :   for (auto proc_name :
+    2559             :        processor_names_) {  // need to access the processors in the specific order from the config (e.g. median filter should go before saturation etc.)
+    2560             :     /* bool is_ok, should_fuse; */
+    2561      648607 :     auto [is_ok, should_fuse] = processors_[proc_name]->process(measurement);
+    2562      648321 :     ok_flag &= is_ok;
+    2563      648321 :     fuse_flag &= should_fuse;
+    2564             :   }
+    2565      737756 :   if (fuse_flag) {
+    2566      721550 :     if (!ok_flag) {
+    2567         289 :       setR(default_R_ * R_coeff_);
+    2568         289 :       ROS_INFO_THROTTLE(1.0, "[%s]: set R to %.4f", getPrintName().c_str(), default_R_ * R_coeff_);
+    2569         289 :       return true;
+    2570             :     } else {
+    2571      721261 :       setR(default_R_);
+    2572      721213 :       return true;
+    2573             :     }
+    2574             :   }
+    2575       16206 :   return false;
+    2576             : }
+    2577             : /*//}*/
+    2578             : 
+    2579             : /*//{ resetProcessors() */
+    2580             : template <int n_measurements>
+    2581           0 : void Correction<n_measurements>::resetProcessors() {
+    2582             : 
+    2583           0 :   for (auto proc_name :
+    2584             :        processor_names_) {  // need to access the processors in the specific order from the config (e.g. median filter should go before saturation etc.)
+    2585             :     /* bool is_ok, should_fuse; */
+    2586           0 :     processors_[proc_name]->reset();
+    2587             :   }
+    2588           0 : }
+    2589             : /*//}*/
+    2590             : 
+    2591             : /*//{ publishCorrection() */
+    2592             : template <int n_measurements>
+    2593     1459507 : void Correction<n_measurements>::publishCorrection(const MeasurementStamped&                                 measurement_stamped,
+    2594             :                                                    mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection>& ph_corr) {
+    2595             : 
+    2596     1459507 :   if (!ch_->debug_topics.correction) {
+    2597           0 :     return;
+    2598             :   }
+    2599             : 
+    2600     2918482 :   mrs_msgs::EstimatorCorrection msg;
+    2601     1458670 :   msg.header.stamp    = measurement_stamped.stamp;
+    2602     1458670 :   msg.header.frame_id = ns_frame_id_;
+    2603     1459269 :   msg.name            = name_;
+    2604     1459362 :   msg.estimator_name  = est_name_;
+    2605     1459387 :   msg.state_id        = state_id_;
+    2606     1459387 :   msg.covariance.resize(n_measurements * n_measurements);
+    2607     3383730 :   for (int i = 0; i < measurement_stamped.value.rows(); i++) {
+    2608     1924813 :     msg.state.push_back(measurement_stamped.value(i));
+    2609     1925797 :     msg.covariance[n_measurements * i + i] = getR();
+    2610             :   }
+    2611             : 
+    2612     1458628 :   ph_corr.publish(msg);
+    2613             : }
+    2614             : /*//}*/
+    2615             : 
+    2616             : /*//{ publishDelay() */
+    2617             : template <int n_measurements>
+    2618             : void Correction<n_measurements>::publishDelay(const double delay) {
+    2619             : 
+    2620             :   if (!ch_->debug_topics.corr_delay) {
+    2621             :     return;
+    2622             :   }
+    2623             : 
+    2624             :   mrs_msgs::Float64Stamped msg;
+    2625             :   msg.header.stamp    = ros::Time::now();
+    2626             :   msg.header.frame_id = ns_frame_id_;
+    2627             :   msg.value           = delay;
+    2628             : 
+    2629             :   ph_delay_.publish(msg);
+    2630             : }
+    2631             : /*//}*/
+    2632             : 
+    2633             : }  // namespace mrs_uav_state_estimators
+    2634             : 
+    2635             : #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..3598933e0b --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.overview.html @@ -0,0 +1,679 @@ + + + + + + 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 + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + 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..2aa384c99b435f81e56de0087d7a840cede4fa62 GIT binary patch literal 6971 zcmV-B8^q*^P)( zRCt{29SNf2Dk!-@dWGf&=@ptAr1!tdyu=NZ%`8pZiGP_+9}fWm5mB4l?bh9Hjout! zY~6N1bDPn$4%>EQImXcT2c$XA0a^JF80p?iKnq5$+bWFahA~!k&4}n|8U=J)m@xuw zrqSFm#vQB`VB7;6##l5n;LQw(fHlB+7DbFTG{(LazzU2vBVcF8SSug_W3{MPblq7x z+)J-f*S2Zc-nCg`=25D_XhPUoM|PG@1n#;WG~r7GZtT6LYb!<@TpiaY$U`ui5SkU& zJTje8!I6glDfli?Gw@~MW8qm9TQa-f6@b>RH9M^bJb;j+Dn(j|^xfyE z*EaW9fv_Tegd_7O&#g4%;sHYHGC=M3y#iuPTt_uj(=?{R1la-;4wl9ea0eO@sar8p z69J7C_bYIXzHIm5x>%NoIpg$8!pN{c&}mF_t2luGK-sQt$$kF;O0z;2Cxo3mA1ClY zlEoBS@GGKRTkZ#|X~eOSG^8jAEp@*xw=B_!LNgij-r_-ySLVtno)v&>oHKv`@iyuJ#?~GBEB6iv3-{0bO#p1s*E}GGD*$s@ zl?x?~fsh9zFm^>+;I?sgZOPA77@JHcjC+9{Xfy(=F+(6ot3KkfD9dG28x2{sEzVpX@#^Q zgSgCLgc;47;BWa)Jz(F@|7IUe7$3>4IJ^)rvDEiN?;X@I%YT^1n#G9G)|f)4`cRD0Z0Knwsb?R}mC@(|;}#l8Dw&Z<@*bCQ;fHWaZ~^cg8s3+V zS@e%y92oG=pJ)QNjSG$dz0nlMG4<18)yJbBW_f3Y-QsHoy#s^IC?PLJL_MqtFLRxie-wpqGfkHJlPzz`zXJvmh;}?PRXTwqSHxBTw&(HyfcFW1Q9>;NthjvG9D*MJ^G*^QF!H9;3}c zW;AjXCK^Q|-$LWxXNktq%@!hR=PSG!-Gp`kGqZmLK8IDIuz*a8O@y3tbAaxL&R|49 zk21H|L{R^LYgaA~1~xRmoz$F4&SL|jQ-N=X&;p9BU{v$IBybarvLFUQcc!4u| zmgjM2mp1_qI3Y?hYX8r83*lPsd_ZAU-z^tH)vdXSWv+i-cknCDv>3lWY zx2lPXq==pJ%sDtFfNWpU+ds= z3sT<9gTw^`1-qofBACF~3~1uJSyr}|V&rF@+v9h9PRlCzJ%F%b;35iQJ+Nj&Od0DB z@MfyNuM6VPY)HBNx>C0~Z>DR{k!2E~Fok5s&b4tn)r_%b-_<6bXYxG)zT|tW<1Gk#=>oIRx! z%M`6D#giFB3UIRK!*^EP=kCdUbF0}+v331K4uD7bnGOIGb2)%daR7OEcsT$W(A*BI z7|M6J%F%zq5(JrRTNG!PQuHo7RYz3AyGFxI9#+r7PYc@A8$E-GlDvEXDQICEEr6?p$0h-Q| z9H3_R-78(pt){@w)UqH>!H6faU^KH==| z7VXG~AxLWNIuDR(NSpbu4@9aBVv54W4?{|R6DH20Z``w3ABA6BeRHOUxrm-2d*UgdOW zPv@kN-@Breo!XO0HRa>_sZq-7l>(L175DR#SGKO7nazk8KhIMXYp$>H6mLi&6C-n_ zka<9JJAY#TFf07t`TE+)0ZxN`R2n6tlsK`Q?F+}jH8=Phz;PSD!AzGHoq zF@BsC<~OEYR>+73a(!)x(SYW5bXC)TRo3|15h*<~S47GKn%kNi23{e_HKoFpDZ6)O z--@9#?<{cVsf{;t&eMnDNSBL5`+gwqO~r>LnaMQ|FUXN`hqL2B?d9JDILb$dzG7s; z)T&9GyvvR%_Fq%tk1-U0A96rMEHhnxde#9EX*@8X|A+!K3eXan;oBs@es_85hqglM zQqVhjb69yq*8}SM(-@<|gwc`8AH=K(;*1Bx8e`8D*JF0!nwM+`ic;KP#fO{Q{*lxbAc@Ru4v;87IY4@izZxL@QrvTeGtMfmaAxWGKhGd#KSQQg zXb;w-^viWLRs={^j7;S>=mN$)VwPKpDWdU2XP;UoA`0U8-L~83Ea6=pdlsy6Qg~ZO2J({`q$m{D$9uhY!*O&wy#v?~AUYQ7@F-`3!9^gr8V_ zJkKkXOjC!POD$F(`&`%h0jm$%%_G1onHq_GAK;aqC9M$fvhNm~3Ghnzip*R);&Rtt zV6NY?BW?z~a?MVOJ|^00_Lt)B3ohiymOtEyn9;^ zih15l%`9AhZvk{k%vAs#mf>^{6>3H)74~jk&9cgS|2w%V*_p8|&FZqQ|NA}{rP&l9 zva#)epC5-`6Z`gd)x_;-T$jzT7=O@at_g_+ly{^Sz(STAc5;C}KEv9>ul&c7*zJ3o z+y?EMJjJyGnUw0cb|3@Km&e`OuFWROqD#P?mjGtKEAC-2e!LZ4NkFNk?2J;YV9w4c z7ly=k)YG!(Bmr%gI6?K4GpHPD4%!;aaKD`?J%T@lSUs*FQzB!;|3iFZj>!s{tEVzn~9qio45g2KN=z zo=mZddo&-0yBcfEwMIOJHEe&@8Zb#*l~e#O03J}p=m9x>TnSiQHZJ4ynGtk2&NwhH?C# z%JI^BmET^D_u2?}RoOY+_~itfz5VcJ{Nau?pm=_UBV8y8)3XHlHjg_(eP2Ry%{JeI%`;L_02FWk%-OcNH-Hpf~%$>W6E6O#oi&>(t%_c*T!SiGJDRTH$`PfMvJc6t;i` zeLehs0Soi2%vHd$%iag4Sa9QXZYy`27o^2m6^!ZbHJbRGiDWl zYr5X2b|&1lx%?B4Nm{XtGtKTdI zb+3L~*r~#ZsAOw2?+&sw>tQRPR`(SFRc7XPE%&5$eu7QOuoX7LDC95NjJ`ngG@?mC znvfo9#i9c=EPgOE>Rfo)^?za4HM@JI6elmkY00Ket_aZ>D|kSY9Ix#H1+aSa6XkpJ zn{18)y!PT)j34h(t}~EQgSkC1i@d!c|Al{2;VHheAfKe=x(H3qA|FvcUtWY(UV~OS z)ayiCuKfLX4pD_Z)E02MrO=y0*n;F%qv9AD8+UzGl>@| zK4Z+4N}R)QKwv6iv;*>fqRvy4yllf5-_huz?`2ZMH6wGKLlOZuEqrqLdcbQv!(#mS z^^Cp!@R5_fY(`⋘z5dSsYM>jF?fK{bkn@#-Fk4EA@G$KDS2zX28p(uoyqyrCg~` zsiB;d_%`1jsZY4x{2x-E)WO&TQlBP-8Ag1Z;^prm@gwt%$y6?J|DI$jl})uX^MlC( zC^Zy{z`WTvgVS-KX~ zEa4kjwr4<-ySI<@lMJpiT~3PYtwk6bwE9pY0Nd2HMsZJCb8!wJ?OmMe0Iu*_YA{L> zUwPgmyhbs}{}5ht%v|C1=9Y)o{44P939pIx+Rn@sUK=o)xW2;cPnYA{^|1-Zs=OoE zi<|D&{S-F|7Emo7SPw z#Uln2we6WtUBaTm*&YCYAD~SipUYc!ls1>}G!~3I&xn(=tsTJC;eLj>?AMj1A8*$JNgeyZFWw z37?L8SBj*}k=U8JQY0G2suW3a#X20-VFE>CfJ3bN<__R1Tv79Vz`IDp+9MT{N$5Iefp z0s6ru%?AHmz?W69DOGIdycy6i{mfbThP)HhA3w7*bHtf5>$H%rvy8&rd@jeGb}``j z11QhM+~U^5Et2j44aZ&nx^DI79h=>c#WenSQFts$+K9*nzMYhtQQp5Q@K(v4AEV*j zPRKmQrVYa=-z(o3#t;DNV>owh8k?IH?q)g(@wDx`R@Ho*x9^&b$FFRs%F5#y-n5+> zez0WvDz;P2^10u$o|tT9V|v-?9y7yBYIIJ}_8i^-IDb0>i8<1MK&oJcT|YF8w%t#= zQe-u@QrG_HB86Zl*QYO|97{m!POhDqT>V%Y6JReD1ZRL!adD>qU=KD2%wOf?FbXIK z5zv7KZ~$l(xDoE&NE(9x&CS2e8Qks;uPif>f6*!u8ddkDjC$F}p^qH+d6&(Oy2i((_YIA&{AyHkfdClky(-oFcAk zuo*7Xcog7$&?Zk5Du8gb=bzpd!_3O3x66`2ILL9VYfA>j<6#hxkH8_IcIxZCu?X?u zGpIhR8a4c()+zX|4gKCfs9I6-6>B^Ntu|46jLpu>z37)48Kh%sWWNB)G@6-FjVl4x z7#CNBivTy8LXYiXIMo!)?JK4@Ycu-Mjr!PQh67-2eYj#@Re*uOS^M~|o0zl_=9!rh-@vtB#M6z39a^oCBOo>Tm+`96->-TjD>8- zM^;gv_>ML3>nrX|72g`nvI?WXyHAmTC7ukZigDX@tv*|_Y*vK4$q~;xeuBAb3&Zu% z9H6dl_T#+#0NR1e?jq_#mrlpu#=E8vvS3_IsDVF(2Jn|K3M88|0PJq_wgW6#m zfd)I>9g%-5bD+Q(9H8M93TfMEj!NN#5Ou7GqY3SybO))K8%8Q5Com2Okike22bIFs z!cBJiJERoXB*qMoT{R#;p63uhMoZwNSbeIp!nl;Y$1cVd_OM6y&9~W%A4kfwVN>v5 zZC$aapjiPlZ#Eq`7plb+n|H*Qg>zEC0}dG5G5VqLMk8#{R$z>avwi@deVV|jUEkLp z(^_Pvthv(kY5*JnIn1vBjQ-TIE2|vB-(e0w>osCDwPyeAl)%j33|bLS6pnn0Rw14q z<@a1-yP{TM_u|LO9NCIrtomjgmVA+Kzzw=@Kjp|xPXjaHd68GyF@Vpllwo zUw83;!jRj$2KFM0-FK(fn>Rs-+9*~iWbVz7%d(<@7?WF6EEn{Hs*&3uwa$}pGnSMb zbwI2&%n{H~f=|SZAZGNg4`QaXSTfB_i+%lvFawl`xurf=voL41u6C&n)ea}BnN^G} z@=&~qO1@z&iEmoP75Rgsu8A@fAsur~R48~L(rRfn9{`sH6;Ai0^un-KJiU~tA+s~_ zSarzJfQ;rDrI$W=g=9w!I>pT7--#PtvMC6(Fs1=3Fmgy$8o_048Aks89uZ^wiQCzVS=O5lRoDbbN4HvDl(#0R}u>dN^L+Ylu zABRs zy)MNfqxlevVzH|k=HKWac~Udo9j4Q0+ij}MUK_jC`@L74wDUqyF@C~E&Thr-tiR2s zikh;WaH2s!&M~Im!pYewoQCip#<{A==m5$yK3vz<3CTRqy!5Em0j?SA-3nL?k;g-F z_Sx^-nJG>KGH&DK>x0oep_EiV%H3lYFiu8XSeSlW*}h7BDb7AoWCBD(6UGW3zg%{} z`hF-URx}Gx=QK$H6@bHxnkYa&^aQOkA$^3iS1L};4uIW@^Uk$$A8tXJW8}Bki~>6D zasw|X9Wd+zup}qJbrP80(pWyp=eg_+j+z8_;)a+xM`~^}0;FjS+&^#mMG6gf=6L`B N002ovPDHLkV1hQXVc`G( literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.func-sort-c.html new file mode 100644 index 0000000000..d87bde2053 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_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/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-12-17 22:35:29Functions: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..04e160a69b --- /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-12-17 22:35:29Functions: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..55e83327d7 --- /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-12-17 22:35:29Functions: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-12-17 22:35:29Functions: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)116
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough()116
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough().2116
+
+
+ + + +
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..a519e4fc1b --- /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-12-17 22:35:29Functions: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)116
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough()116
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough().2116
+
+
+ + + +
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..42621f58ee --- /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-12-17 22:35:29Functions: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         116 :   HdgPassthrough(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name, const bool is_core_plugin)
+      70         116 :       : HeadingEstimator<hdg_passthrough::n_states>(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), is_core_plugin_(is_core_plugin) {
+      71         116 :   }
+      72             : 
+      73         232 :   ~HdgPassthrough(void) {
+      74         232 :   }
+      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..516d50d098 --- /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-12-17 22:35:29Functions: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&)116
+
+
+ + + +
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..1a773fa27f --- /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-12-17 22:35:29Functions: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&)116
+
+
+ + + +
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..7fc8402c18 --- /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-12-17 22:35:29Functions: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         116 :   HeadingEstimator(const std::string& name, const std::string& frame_id) : PartialEstimator<n_states, 1>(heading::type, name, frame_id) {
+      23         116 :   }
+      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..836ad74867 --- /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-12-17 22:35:29Functions: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..defbb46643 --- /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-12-17 22:35:29Functions: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..76bc4990ff --- /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-12-17 22:35:29Functions: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..5cdc0e75f4 --- /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-12-17 22:35:29Functions: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..148a52ac8a --- /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-12-17 22:35:29Functions: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..e68011506b --- /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-12-17 22:35:29Functions: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..034b3c2f79 --- /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:416112537.0 %
Date:2024-12-17 22:35:29Functions:6711160.4 %
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 +
35.1%35.1%
+ 		35.1 %382 / 108955.2 %48 / 87
<unnamed>35.1 %382 / 108955.2 %48 / 87
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..5c22ba48ec --- /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:416112537.0 %
Date:2024-12-17 22:35:29Functions:6711160.4 %
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 +
35.1%35.1%
+ 		35.1 %382 / 108955.2 %48 / 87
<unnamed>35.1 %382 / 108955.2 %48 / 87
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..d19ce622f8 --- /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:416112537.0 %
Date:2024-12-17 22:35:29Functions:6711160.4 %
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 +
35.1%35.1%
+ 		35.1 %382 / 108955.2 %48 / 87
<unnamed>35.1 %382 / 108955.2 %48 / 87
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..6023e8824d --- /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:416112537.0 %
Date:2024-12-17 22:35:29Functions:6711160.4 %
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 +
35.1%35.1%
+ 		35.1 %382 / 108955.2 %48 / 87
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..bad8dbdeda --- /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:416112537.0 %
Date:2024-12-17 22:35:29Functions:6711160.4 %
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 +
35.1%35.1%
+ 		35.1 %382 / 108955.2 %48 / 87
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..5bcbbb39ab --- /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:416112537.0 %
Date:2024-12-17 22:35:29Functions:6711160.4 %
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 +
35.1%35.1%
+ 		35.1 %382 / 108955.2 %48 / 87
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..c6201f4b61 --- /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-12-17 22:35:29Functions: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..afea661595 --- /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-12-17 22:35:29Functions: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..145d5f430f --- /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-12-17 22:35:29Functions: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..e509e0dba2 --- /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-12-17 22:35:29Functions: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..5e6dd39795 --- /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-12-17 22:35:29Functions: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..721cdc0025 --- /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-12-17 22:35:29Functions: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..f36d8c874e --- /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-12-17 22:35:29Functions: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> ()>)116
mrs_uav_state_estimators::LatGeneric::~LatGeneric()116
mrs_uav_state_estimators::LatGeneric::~LatGeneric().2116
+
+
+ + + +
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..1d38f85aca --- /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-12-17 22:35:29Functions: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> ()>)116
mrs_uav_state_estimators::LatGeneric::~LatGeneric()116
mrs_uav_state_estimators::LatGeneric::~LatGeneric().2116
+
+
+ + + +
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..ca978f9245 --- /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-12-17 22:35:29Functions: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         116 :   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         116 :       : 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         116 :         fun_get_hdg_(fun_get_hdg) {
+     124         116 :   }
+     125             : 
+     126         232 :   ~LatGeneric(void) {
+     127         232 :   }
+     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-12-17 22:35:29Functions: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&)116
mrs_uav_state_estimators::LateralEstimator<6>::~LateralEstimator().2116
+
+
+ + + +
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..6f2dab89ad --- /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-12-17 22:35:29Functions: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&)116
mrs_uav_state_estimators::LateralEstimator<6>::~LateralEstimator()0
mrs_uav_state_estimators::LateralEstimator<6>::~LateralEstimator().2116
+
+
+ + + +
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..25d222960c --- /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-12-17 22:35:29Functions: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         116 :   LateralEstimator(const std::string& name, const std::string& frame_id) : PartialEstimator<n_states, lateral::n_axes>(lateral::type, name, frame_id) {
+      24         116 :   }
+      25             : 
+      26         116 :   ~LateralEstimator(void) {
+      27         116 :   }
+      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..d19e528b36 --- /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-12-17 22:35:29Functions: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&)116
mrs_uav_state_estimators::PartialEstimator<2, 1>::~PartialEstimator().2116
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&)116
mrs_uav_state_estimators::PartialEstimator<6, 2>::~PartialEstimator().2116
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&)196
mrs_uav_state_estimators::PartialEstimator<3, 1>::~PartialEstimator().2196
mrs_uav_state_estimators::PartialEstimator<3, 1>::stateIdToIndex(int const&, int const&) const141919
mrs_uav_state_estimators::PartialEstimator<6, 2>::getCovarianceAsVector() const198450
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&) const198456
mrs_uav_state_estimators::PartialEstimator<6, 2>::publishOutput() const198459
mrs_uav_state_estimators::PartialEstimator<6, 2>::getStatesAsVector() const198464
mrs_uav_state_estimators::PartialEstimator<2, 1>::getCovarianceAsVector() const218277
mrs_uav_state_estimators::PartialEstimator<2, 1>::getStatesAsVector() const218291
mrs_uav_state_estimators::PartialEstimator<2, 1>::publishOutput() const218299
mrs_uav_state_estimators::PartialEstimator<3, 1>::getCovarianceAsVector() const312258
mrs_uav_state_estimators::PartialEstimator<3, 1>::publishOutput() const312659
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&) const312686
mrs_uav_state_estimators::PartialEstimator<3, 1>::getStatesAsVector() const312760
mrs_uav_state_estimators::PartialEstimator<6, 2>::stateIdToIndex(int const&, int const&) const2968988
+
+
+ + + +
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..cb16210108 --- /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-12-17 22:35:29Functions: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&)116
mrs_uav_state_estimators::PartialEstimator<2, 1>::~PartialEstimator()0
mrs_uav_state_estimators::PartialEstimator<2, 1>::~PartialEstimator().2116
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&)196
mrs_uav_state_estimators::PartialEstimator<3, 1>::~PartialEstimator()0
mrs_uav_state_estimators::PartialEstimator<3, 1>::~PartialEstimator().2196
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&)116
mrs_uav_state_estimators::PartialEstimator<6, 2>::~PartialEstimator()0
mrs_uav_state_estimators::PartialEstimator<6, 2>::~PartialEstimator().2116
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() const218299
mrs_uav_state_estimators::PartialEstimator<2, 1>::stateIdToIndex(int const&, int const&) const0
mrs_uav_state_estimators::PartialEstimator<2, 1>::getStatesAsVector() const218291
mrs_uav_state_estimators::PartialEstimator<2, 1>::getCovarianceAsVector() const218277
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&) const312686
mrs_uav_state_estimators::PartialEstimator<3, 1>::publishOutput() const312659
mrs_uav_state_estimators::PartialEstimator<3, 1>::stateIdToIndex(int const&, int const&) const141919
mrs_uav_state_estimators::PartialEstimator<3, 1>::getStatesAsVector() const312760
mrs_uav_state_estimators::PartialEstimator<3, 1>::getCovarianceAsVector() const312258
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&) const198456
mrs_uav_state_estimators::PartialEstimator<6, 2>::publishOutput() const198459
mrs_uav_state_estimators::PartialEstimator<6, 2>::stateIdToIndex(int const&, int const&) const2968988
mrs_uav_state_estimators::PartialEstimator<6, 2>::getStatesAsVector() const198464
mrs_uav_state_estimators::PartialEstimator<6, 2>::getCovarianceAsVector() const198450
+
+
+ + + +
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..e36b010abe --- /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-12-17 22:35:29Functions: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         428 :   PartialEstimator(const std::string &type, const std::string &name, const std::string &frame_id) : Estimator(type, name, frame_id) {
+      62         428 :   }
+      63             : 
+      64         428 :   ~PartialEstimator(void) {
+      65         428 :   }
+      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      729515 : std::vector<double> PartialEstimator<n_states, n_axes>::getStatesAsVector(void) const {
+     103      729515 :   const states_t      states = getStates();
+     104      728920 :   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     3292680 :   for (int i = 0; i < states.size(); i++) {
+     109     2562293 :     states_vec.push_back(states(i));
+     110             :   }
+     111     1457416 :   return states_vec;
+     112             : }
+     113             : /*//}*/
+     114             : 
+     115             : /*//{ getCovarianceAsvector() */
+     116             : template <int n_states, int n_axes>
+     117      728985 : std::vector<double> PartialEstimator<n_states, n_axes>::getCovarianceAsVector(void) const {
+     118      728985 :   const covariance_t  covariance = getCovarianceMatrix();
+     119      728949 :   std::vector<double> covariance_vec;
+     120             :   /* for (auto cov : covariance.reshaped<Eigen::RowMajor>(covariance.size())) { */
+     121             :   /*   covariance_vec.push_back(*cov); */
+     122             :   /* } */
+     123     3291124 :   for (int i = 0; i < covariance.rows(); i++) {
+     124    13389746 :     for (int j = 0; j < covariance.cols(); j++) {
+     125    10825147 :       covariance_vec.push_back(covariance(i, j));
+     126             :     }
+     127             :   }
+     128     1457760 :   return covariance_vec;
+     129             : }
+     130             : /*//}*/
+     131             : 
+     132             : /*//{ stateIdToIndex() */
+     133             : template <int n_states, int n_axes>
+     134     3110907 : int PartialEstimator<n_states, n_axes>::stateIdToIndex(const int &state_id_in, const int &axis_in) const {
+     135     3110907 :   return state_id_in * _n_axes_ + axis_in;
+     136             : }
+     137             : /*//}*/
+     138             : 
+     139             : /*//{ publishOutput() */
+     140             : template <int n_states, int n_axes>
+     141      729417 : void PartialEstimator<n_states, n_axes>::publishOutput() const {
+     142             : 
+     143      729417 :   if (!ch_->debug_topics.output) {
+     144           0 :     return;
+     145             :   }
+     146             : 
+     147     1458678 :   mrs_msgs::EstimatorOutput msg;
+     148      728715 :   msg.header.stamp    = ros::Time::now();
+     149      729587 :   msg.header.frame_id = getFrameId();
+     150      729494 :   msg.state           = getStatesAsVector();
+     151      728067 :   msg.covariance      = getCovarianceAsVector();
+     152             : 
+     153      728703 :   ph_output_.publish(msg);
+     154             : }
+     155             : /*//}*/
+     156             : 
+     157             : /*//{ publishInput() */
+     158             : template <int n_states, int n_axes>
+     159             : template <typename u_t>
+     160      511142 : void PartialEstimator<n_states, n_axes>::publishInput(const u_t &u, const ros::Time& stamp) const {
+     161             : 
+     162      511142 :   if (!ch_->debug_topics.input) {
+     163           0 :     return;
+     164             :   }
+     165             : 
+     166     1018919 :   mrs_msgs::Float64ArrayStamped msg;
+     167      508125 :   msg.header.stamp = stamp;
+     168     1216210 :   for (int i = 0; i < u.rows(); i++) {
+     169      705649 :     msg.values.push_back(u(i));
+     170             :   }
+     171             : 
+     172      508852 :   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-12-17 22:35:29Functions: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..12ca1acce2 --- /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-12-17 22:35:29Functions: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..33f96e1d28 --- /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-12-17 22:35:29Functions: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..435c9e9080 --- /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-12-17 22:35:29Functions: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..6a1c009013 --- /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-12-17 22:35:29Functions: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..c8b7fe2626 --- /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-12-17 22:35:29Functions: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..c32d12b778 --- /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-12-17 22:35:29Functions: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..d8134b0b17 --- /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-12-17 22:35:29Functions: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..5489c8b949 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.html @@ -0,0 +1,179 @@ + + + + + + + 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-12-17 22:35:29Functions: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             :   void                                          callbackPassthroughOdom(const nav_msgs::Odometry::ConstPtr msg);
+      52             :   double                                        _critical_timeout_passthrough_odom_;
+      53             :   std::string                                   msg_topic_;
+      54             : 
+      55             :   ros::Timer       timer_check_passthrough_odom_hz_;
+      56             :   void             timerCheckPassthroughOdomHz(const ros::TimerEvent &event);
+      57             :   std::atomic<int> counter_odom_msgs_ = 0;
+      58             :   ros::WallTime    t_check_hz_last_;
+      59             :   double           prev_avg_hz_ = 0;
+      60             :   bool             kickoff_     = false;
+      61             : 
+      62             :   ros::Timer                 timer_update_;
+      63             :   void                       timerUpdate(const ros::TimerEvent &event);
+      64             :   nav_msgs::OdometryConstPtr prev_msg_;
+      65             :   bool                       first_iter_ = true;
+      66             : 
+      67             :   bool isConverged();
+      68             : 
+      69             :   void waitForEstimationInitialization();
+      70             : 
+      71             : public:
+      72           1 :   Passthrough() : StateEstimator(passthrough::name, passthrough::frame_id, passthrough::package_name), is_core_plugin_(is_core_plugin) {
+      73           1 :   }
+      74             : 
+      75           2 :   ~Passthrough(void) {
+      76           2 :   }
+      77             : 
+      78             :   void initialize(ros::NodeHandle &parent_nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) override;
+      79             :   bool start(void) override;
+      80             :   bool pause(void) override;
+      81             :   bool reset(void) override;
+      82             : 
+      83             :   /* mrs_msgs::UavState  getUavState() override; */
+      84             :   /* nav_msgs::Odometry  getInnovation() const override; */
+      85             :   /* std::vector<double> getPoseCovariance() const override; */
+      86             :   /* std::vector<double> getTwistCovariance() const override; */
+      87             : 
+      88             :   bool setUavState(const mrs_msgs::UavState &uav_state) override;
+      89             : };
+      90             : 
+      91             : }  // namespace passthrough
+      92             : 
+      93             : }  // namespace mrs_uav_state_estimators
+      94             : 
+      95             : #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..2c9da25653 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.overview.html @@ -0,0 +1,44 @@ + + + + + + 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 + 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..08343947479917b7e9045638d31caaa211daee60 GIT binary patch literal 460 zcmV;-0W + + + + + + 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-12-17 22:35:29Functions: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)116
mrs_uav_state_estimators::StateGeneric::~StateGeneric().2116
+
+
+ + + +
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..2a6f49b07c --- /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-12-17 22:35:29Functions: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)116
mrs_uav_state_estimators::StateGeneric::~StateGeneric()0
mrs_uav_state_estimators::StateGeneric::~StateGeneric().2116
+
+
+ + + +
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..880e021e72 --- /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-12-17 22:35:29Functions: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         116 :   StateGeneric(const std::string &name, const bool is_core_plugin)
+      79         116 :       : StateEstimator(name, name + "_origin", state_generic::package_name), is_core_plugin_(is_core_plugin) {
+      80         116 :   }
+      81             : 
+      82         116 :   ~StateGeneric(void) {
+      83         116 :   }
+      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..a30d1c8191 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail-sort-f.html @@ -0,0 +1,192 @@ + + + + + + + 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:13323656.4 %
Date:2024-12-17 22:35:29Functions:174240.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
proc_mag_declination.h +
0.0%
+ 		0.0 %0 / 520.0 %0 / 8
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 +
70.7%70.7%
+ 		70.7 %29 / 4133.3 %2 / 6
<unnamed>70.7 %29 / 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..f721e9b344 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail-sort-l.html @@ -0,0 +1,192 @@ + + + + + + + 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:13323656.4 %
Date:2024-12-17 22:35:29Functions:174240.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
proc_mag_declination.h +
0.0%
+ 		0.0 %0 / 520.0 %0 / 8
proc_median_filter.h +
63.9%63.9%
+ 		63.9 %23 / 3633.3 %2 / 6
<unnamed>63.9 %23 / 3633.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_excessive_tilt.h +
70.7%70.7%
+ 		70.7 %29 / 4133.3 %2 / 6
<unnamed>70.7 %29 / 4133.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
+
+
+ + + + +
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..d068105a65 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail.html @@ -0,0 +1,192 @@ + + + + + + + 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:13323656.4 %
Date:2024-12-17 22:35:29Functions:174240.5 %
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 +
70.7%70.7%
+ 		70.7 %29 / 4133.3 %2 / 6
<unnamed>70.7 %29 / 4133.3 %2 / 6
proc_mag_declination.h +
0.0%
+ 		0.0 %0 / 520.0 %0 / 8
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..fafb2387b8 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-sort-f.html @@ -0,0 +1,152 @@ + + + + + + + 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:13323656.4 %
Date:2024-12-17 22:35:29Functions:174240.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
proc_mag_declination.h +
0.0%
+ 		0.0 %0 / 520.0 %0 / 8
proc_median_filter.h +
63.9%63.9%
+ 		63.9 %23 / 3633.3 %2 / 6
proc_excessive_tilt.h +
70.7%70.7%
+ 		70.7 %29 / 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..7508643c71 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-sort-l.html @@ -0,0 +1,152 @@ + + + + + + + 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:13323656.4 %
Date:2024-12-17 22:35:29Functions:174240.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
proc_mag_declination.h +
0.0%
+ 		0.0 %0 / 520.0 %0 / 8
proc_median_filter.h +
63.9%63.9%
+ 		63.9 %23 / 3633.3 %2 / 6
processor.h +
70.0%70.0%
+ 		70.0 %7 / 1075.0 %6 / 8
proc_excessive_tilt.h +
70.7%70.7%
+ 		70.7 %29 / 4133.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
+
+
+ + + + +
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..fc0d1f66dd --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index.html @@ -0,0 +1,152 @@ + + + + + + + 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:13323656.4 %
Date:2024-12-17 22:35:29Functions:174240.5 %
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 +
70.7%70.7%
+ 		70.7 %29 / 4133.3 %2 / 6
proc_mag_declination.h +
0.0%
+ 		0.0 %0 / 520.0 %0 / 8
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/mag_declination/geo_mag_declination.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h.func-sort-c.html new file mode 100644 index 0000000000..301f1df875 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h.func-sort-c.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination - geo_mag_declination.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:0260.0 %
Date:2024-12-17 22:35:29Functions:040.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::GeoMagDeclination::getTableData(float, float, short const (*) [37])0
mrs_uav_state_estimators::GeoMagDeclination::getLookupTableIndex(float*, float, float)0
mrs_uav_state_estimators::GeoMagDeclination::getMagDeclinationRadians(float, float)0
float mrs_uav_state_estimators::GeoMagDeclination::constrain<float>(float, float, float)0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h.func.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h.func.html new file mode 100644 index 0000000000..496deb4fe0 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h.func.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination - geo_mag_declination.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:0260.0 %
Date:2024-12-17 22:35:29Functions:040.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::GeoMagDeclination::getTableData(float, float, short const (*) [37])0
mrs_uav_state_estimators::GeoMagDeclination::getLookupTableIndex(float*, float, float)0
mrs_uav_state_estimators::GeoMagDeclination::getMagDeclinationRadians(float, float)0
float mrs_uav_state_estimators::GeoMagDeclination::constrain<float>(float, float, float)0
+
+
+ + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h.gcov.frameset.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h.gcov.frameset.html new file mode 100644 index 0000000000..70b138dc5a --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.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/mag_declination/geo_mag_declination.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h.gcov.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h.gcov.html new file mode 100644 index 0000000000..5faf3e12b2 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h.gcov.html @@ -0,0 +1,174 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination - geo_mag_declination.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:0260.0 %
Date:2024-12-17 22:35:29Functions:040.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #pragma once
+       2             : #ifndef GEO_MAG_DECLINATION_H
+       3             : #define GEO_MAG_DECLINATION_H
+       4             : 
+       5             : #include "geo_magnetic_tables.hpp"
+       6             : 
+       7             : #include <math.h>
+       8             : #include <stdint.h>
+       9             : 
+      10             : namespace mrs_uav_state_estimators
+      11             : {
+      12             : 
+      13             : class GeoMagDeclination {
+      14             : 
+      15             : public:
+      16             :   /*//{ constrain() */
+      17             :   template <typename T>
+      18           0 :   static T constrain(T val, T min_val, T max_val) {
+      19           0 :     return (val < min_val) ? min_val : ((val > max_val) ? max_val : val);
+      20             :   }
+      21             :   /*//}*/
+      22             : 
+      23             :   /*//{ degrees() */
+      24             :   template <typename T>
+      25             :   static T degrees(T radians) {
+      26             :     return radians * (static_cast<T>(180) / static_cast<T>(M_PI));
+      27             :   }
+      28             :   /*//}*/
+      29             : 
+      30             :   /*//{ getLookupTableIndex() */
+      31           0 :   static unsigned getLookupTableIndex(float *val, float min, float max) {
+      32           0 :     *val = GeoMagDeclination::constrain(*val, min, max - SAMPLING_RES);
+      33             : 
+      34           0 :     return static_cast<unsigned>((-(min) + *val) / SAMPLING_RES);
+      35             :   }
+      36             :   /*//}*/
+      37             : 
+      38             :   /*//{ getTableData() */
+      39           0 :   static float getTableData(float lat, float lon, const int16_t table[LAT_DIM][LON_DIM]) {
+      40             : 
+      41           0 :     lat = GeoMagDeclination::constrain(lat, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT);
+      42             : 
+      43           0 :     if (lon > SAMPLING_MAX_LON) {
+      44           0 :       lon -= 360;
+      45             :     }
+      46             : 
+      47           0 :     if (lon < SAMPLING_MIN_LON) {
+      48           0 :       lon += 360;
+      49             :     }
+      50             : 
+      51             :     /* round down to nearest sampling resolution */
+      52           0 :     float min_lat = floorf(lat / SAMPLING_RES) * SAMPLING_RES;
+      53           0 :     float min_lon = floorf(lon / SAMPLING_RES) * SAMPLING_RES;
+      54             : 
+      55             :     /* find index of nearest low sampling point */
+      56           0 :     unsigned min_lat_index = GeoMagDeclination::getLookupTableIndex(&min_lat, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT);
+      57           0 :     unsigned min_lon_index = GeoMagDeclination::getLookupTableIndex(&min_lon, SAMPLING_MIN_LON, SAMPLING_MAX_LON);
+      58             : 
+      59           0 :     const float data_sw = table[min_lat_index][min_lon_index];
+      60           0 :     const float data_se = table[min_lat_index][min_lon_index + 1];
+      61           0 :     const float data_ne = table[min_lat_index + 1][min_lon_index + 1];
+      62           0 :     const float data_nw = table[min_lat_index + 1][min_lon_index];
+      63             : 
+      64             :     /* perform bilinear interpolation on the four grid corners */
+      65           0 :     const float lat_scale = GeoMagDeclination::constrain((lat - min_lat) / SAMPLING_RES, 0.f, 1.f);
+      66           0 :     const float lon_scale = GeoMagDeclination::constrain((lon - min_lon) / SAMPLING_RES, 0.f, 1.f);
+      67             : 
+      68           0 :     const float data_min = lon_scale * (data_se - data_sw) + data_sw;
+      69           0 :     const float data_max = lon_scale * (data_ne - data_nw) + data_nw;
+      70             : 
+      71           0 :     return lat_scale * (data_max - data_min) + data_min;
+      72             :   }
+      73             :   /*//}*/
+      74             : 
+      75           0 :   static float getMagDeclinationRadians(float lat, float lon) {
+      76           0 :     return GeoMagDeclination::getTableData(lat, lon, declination_table) * 1e-4f;  // declination table stored as 10^-4 radians
+      77             :   }
+      78             : 
+      79             :   static float getMagDeclinationDegrees(float lat, float lon) {
+      80             :     return GeoMagDeclination::degrees(GeoMagDeclination::getMagDeclinationRadians(lat, lon));
+      81             :   }
+      82             : 
+      83             : private:
+      84             :   GeoMagDeclination(){};
+      85             : };
+      86             : 
+      87             : }  // namespace mrs_uav_state_estimators
+      88             : 
+      89             : 
+      90             : #endif
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h.gcov.overview.html new file mode 100644 index 0000000000..b612a78639 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h.gcov.overview.html @@ -0,0 +1,43 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.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/processors/mag_declination/geo_mag_declination.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..fa909f4e5808a262dd7216ba6519a9a62cfd547d GIT binary patch literal 494 zcmV zR7i={R|$^8APfvYfIqTumM;9^NBoL@Wg>3IbsN2#^OHIcrLN z5SJcs5!Ek$gQsmPNS>uSW_c`bpC|zRRhkdF zDabt|v+&nI?lGDo_uK+(v>xuC*lcMZ>ZJ_ARPma$EU!x*091f!;EPy9Ougxj0gvFrT}HJ+{rbq9!T7vSS*a!7S}-!SjZm)~WLW}*$P#+bN9s&d{5u2qOC;RHYr z(O4sOEgBzSC7!js6YpWCE{P(dM{wzeOZsaL!r%>&`e}jlq3a>~1*aLj4|N*4iH_7* zR6Kf)#J}6@^&U<_%VSVC^S~6-<7mc==CR>&pU7lkytU~60MGcqwfXIiJ~Q>D>5;dg ksu3~J_0d|wK3FsT07b9yUMb=7vj6}907*qoM6N<$g8JXsmjD0& literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/index-detail-sort-f.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/index-detail-sort-f.html new file mode 100644 index 0000000000..4b8be07951 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/index-detail-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declinationHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:0260.0 %
Date:2024-12-17 22:35:29Functions: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
geo_mag_declination.h +
0.0%
+ 		0.0 %0 / 260.0 %0 / 4
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/index-detail-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/index-detail-sort-l.html new file mode 100644 index 0000000000..dd2410c413 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/index-detail-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declinationHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:0260.0 %
Date:2024-12-17 22:35:29Functions: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
geo_mag_declination.h +
0.0%
+ 		0.0 %0 / 260.0 %0 / 4
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/index-detail.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/index-detail.html new file mode 100644 index 0000000000..7a12533218 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/index-detail.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declinationHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:0260.0 %
Date:2024-12-17 22:35:29Functions: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
geo_mag_declination.h +
0.0%
+ 		0.0 %0 / 260.0 %0 / 4
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/index-sort-f.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/index-sort-f.html new file mode 100644 index 0000000000..be43bcec30 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declinationHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:0260.0 %
Date:2024-12-17 22:35:29Functions: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
geo_mag_declination.h +
0.0%
+ 		0.0 %0 / 260.0 %0 / 4
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/index-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/index-sort-l.html new file mode 100644 index 0000000000..7fb56586b1 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declinationHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:0260.0 %
Date:2024-12-17 22:35:29Functions: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
geo_mag_declination.h +
0.0%
+ 		0.0 %0 / 260.0 %0 / 4
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/index.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/index.html new file mode 100644 index 0000000000..60f0621c2e --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declination + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/mag_declinationHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:0260.0 %
Date:2024-12-17 22:35:29Functions: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
geo_mag_declination.h +
0.0%
+ 		0.0 %0 / 260.0 %0 / 4
+
+
+ + + + +
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..7ee19918f6 --- /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:294170.7 %
Date:2024-12-17 22:35:29Functions: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&)84
mrs_uav_state_estimators::ProcExcessiveTilt<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)140115
+
+
+ + + +
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..dd1778a0ec --- /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:294170.7 %
Date:2024-12-17 22:35:29Functions: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>&)140115
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&)84
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..afb5ec1abe --- /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:294170.7 %
Date:2024-12-17 22:35:29Functions: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          84 : 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          84 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph) {
+      45             : 
+      46             :   // | --------------------- load parameters -------------------- |
+      47          84 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      48             : 
+      49          84 :   ph->param_loader->loadParam("orientation_topic", orientation_topic_);
+      50             :   double max_tilt;
+      51          84 :   ph->param_loader->loadParam("max_tilt", max_tilt);
+      52             : 
+      53          84 :   max_tilt = M_PI * (max_tilt / 180.0);
+      54             : 
+      55          84 :   max_tilt_sq_ = std::pow(max_tilt, 2);
+      56             : 
+      57          84 :   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          84 :   mrs_lib::SubscribeHandlerOptions shopts;
+      64          84 :   shopts.nh                 = nh;
+      65          84 :   shopts.node_name          = Processor<n_measurements>::getPrintName();
+      66          84 :   shopts.no_message_timeout = ros::Duration(1.0);
+      67          84 :   shopts.threadsafe         = true;
+      68          84 :   shopts.autostart          = true;
+      69          84 :   shopts.queue_size         = 10;
+      70          84 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      71             : 
+      72          84 :   sh_orientation_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, "/" + ch->uav_name + "/" + orientation_topic_);
+      73          84 : }
+      74             : /*//}*/
+      75             : 
+      76             : /*//{ process() */
+      77             : template <int n_measurements>
+      78      140115 : std::tuple<bool, bool> ProcExcessiveTilt<n_measurements>::process([[maybe_unused]] measurement_t& measurement) {
+      79             : 
+      80      140115 :   if (!Processor<n_measurements>::enabled_) {
+      81           0 :     return {true, true};
+      82             :   }
+      83             : 
+      84      140115 :   if (!sh_orientation_.hasMsg()) {
+      85           0 :     return {false, false};
+      86             :   }
+      87             : 
+      88      140112 :   bool ok_flag     = true;
+      89      140112 :   bool should_fuse = true;
+      90             : 
+      91             :   try {
+      92      140112 :     Eigen::Matrix3d orientation_R = mrs_lib::AttitudeConverter(sh_orientation_.getMsg()->quaternion);
+      93             : 
+      94      140111 :     const double tilt = mrs_lib::geometry::angleBetween(Eigen::Vector3d(0, 0, 1), orientation_R.col(2));
+      95             : 
+      96      140100 :     const bool is_excessive_tilt = std::pow(tilt, 2) > max_tilt_sq_;
+      97             : 
+      98      140096 :     if (is_excessive_tilt) {
+      99           5 :       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      140101 :   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..a9577410cc42d06a4fd9ffb2a9d3eb6305b60062 GIT binary patch literal 661 zcmV;G0&4wJ+@1l& z<{5NJ`G~q?Gz+f9z%J=>jL(QF#)p9wj9IaJ!1r9V1bVs~xz?hrMMzc0dY2BAh%5QIH zV6A^Kle_LP@{ECi;o2{ZQK1sJ6r)QJ0!=tW8@R-HFwx*Sqb-vT9xC8@e$g)fy1pNG zr1yCC7Z~synmRvY009@qF!dwrM19YGzaMS8K_ZYO?x?-((YCAY;dO z?Rp9XX)=MdP6h}lG-8CdGGwY3Q^nYjtPPYmPx>S^2QKdLd=WRYUeK{Y1ho)x3a>bl z|2&0djMXkFV+?=)9V3psU!`NgF`Fq+T0(WNp-GH1G~?M>=$<{2+0h(YSZAbyrzaE8 zf2;EBynTADgN!_ClnFutK10m(aBb%M%(c*D77*y5bD*pt<=^+tFsYv@Eql7+^A=~^ zpVBCO=`lP{niywEFH9kQz literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_mag_declination.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_mag_declination.h.func-sort-c.html new file mode 100644 index 0000000000..439696d186 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_mag_declination.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_mag_declination.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_mag_declination.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:0520.0 %
Date:2024-12-17 22:35:29Functions:080.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::ProcMagDeclination<1>::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)0
mrs_uav_state_estimators::ProcMagDeclination<1>::reset()0
mrs_uav_state_estimators::ProcMagDeclination<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)0
mrs_uav_state_estimators::ProcMagDeclination<1>::ProcMagDeclination(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::ProcMagDeclination<2>::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)0
mrs_uav_state_estimators::ProcMagDeclination<2>::reset()0
mrs_uav_state_estimators::ProcMagDeclination<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)0
mrs_uav_state_estimators::ProcMagDeclination<2>::ProcMagDeclination(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_mag_declination.h.func.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_mag_declination.h.func.html new file mode 100644 index 0000000000..02c2f07a1e --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_mag_declination.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_mag_declination.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_mag_declination.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:0520.0 %
Date:2024-12-17 22:35:29Functions:080.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::ProcMagDeclination<1>::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)0
mrs_uav_state_estimators::ProcMagDeclination<1>::reset()0
mrs_uav_state_estimators::ProcMagDeclination<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)0
mrs_uav_state_estimators::ProcMagDeclination<1>::ProcMagDeclination(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::ProcMagDeclination<2>::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)0
mrs_uav_state_estimators::ProcMagDeclination<2>::reset()0
mrs_uav_state_estimators::ProcMagDeclination<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)0
mrs_uav_state_estimators::ProcMagDeclination<2>::ProcMagDeclination(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_mag_declination.h.gcov.frameset.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_mag_declination.h.gcov.frameset.html new file mode 100644 index 0000000000..8cbfd90a11 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_mag_declination.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_mag_declination.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_mag_declination.h.gcov.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_mag_declination.h.gcov.html new file mode 100644 index 0000000000..be752f0851 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_mag_declination.h.gcov.html @@ -0,0 +1,231 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_mag_declination.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_mag_declination.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:0520.0 %
Date:2024-12-17 22:35:29Functions:080.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : #pragma once
+       2             : #ifndef PROCESSORS_MAG_DECLINATION_H
+       3             : #define PROCESSORS_MAG_DECLINATION_H
+       4             : 
+       5             : #include <mrs_uav_state_estimators/processors/processor.h>
+       6             : #include <mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h>
+       7             : 
+       8             : #include <sensor_msgs/NavSatFix.h>
+       9             : 
+      10             : #include <mrs_lib/gps_conversions.h>
+      11             : #include <mrs_lib/subscribe_handler.h>
+      12             : #include <mrs_lib/param_loader.h>
+      13             : 
+      14             : namespace mrs_uav_state_estimators
+      15             : {
+      16             : 
+      17             : using namespace mrs_uav_managers::estimation_manager;
+      18             : 
+      19             : template <int n_measurements>
+      20             : class ProcMagDeclination : public Processor<n_measurements> {
+      21             : 
+      22             : public:
+      23             :   typedef Eigen::Matrix<double, n_measurements, 1> measurement_t;
+      24             : 
+      25             : public:
+      26             :   ProcMagDeclination(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name, const std::shared_ptr<CommonHandlers_t>& ch,
+      27             :                      const std::shared_ptr<PrivateHandlers_t>& ph);
+      28             : 
+      29             :   std::tuple<bool, bool> process(measurement_t& measurement) override;
+      30             :   void                   reset();
+      31             : 
+      32             : private:
+      33             :   bool is_initialized_ = false;
+      34             : 
+      35             :   bool        use_gnss_msg_;
+      36             :   std::string gnss_topic_;
+      37             : 
+      38             :   std::mutex       mtx_gnss_;
+      39             :   std::atomic_bool got_gnss_ = false;
+      40             :   double           gnss_lat_;
+      41             :   double           gnss_lon_;
+      42             : 
+      43             :   mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix> sh_gnss_;
+      44             :   void                                              callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg);
+      45             : };
+      46             : 
+      47             : /*//{ constructor */
+      48             : template <int n_measurements>
+      49           0 : ProcMagDeclination<n_measurements>::ProcMagDeclination(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name,
+      50             :                                              const std::shared_ptr<CommonHandlers_t>& ch, const std::shared_ptr<PrivateHandlers_t>& ph)
+      51           0 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph) {
+      52             : 
+      53           0 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      54             : 
+      55           0 :   ph->param_loader->loadParam("use_gnss_msg", use_gnss_msg_);
+      56           0 :   if (use_gnss_msg_) {
+      57           0 :     ph->param_loader->loadParam("gnss_topic", gnss_topic_);
+      58             :   } else {
+      59           0 :     ph->param_loader->loadParam("latitude", gnss_lat_);
+      60           0 :     ph->param_loader->loadParam("longitude", gnss_lon_);
+      61             :   }
+      62             : 
+      63           0 :   gnss_topic_ = "/" + ch->uav_name + "/" + gnss_topic_;
+      64             : 
+      65           0 :   if (!ph->param_loader->loadedSuccessfully()) {
+      66           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", Processor<n_measurements>::getPrintName().c_str());
+      67           0 :     ros::shutdown();
+      68             :   }
+      69             : 
+      70             :   // | -------------- initialize subscribe handlers ------------- |
+      71           0 :   mrs_lib::SubscribeHandlerOptions shopts;
+      72           0 :   shopts.nh                 = nh;
+      73           0 :   shopts.node_name          = Processor<n_measurements>::getPrintName();
+      74           0 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      75           0 :   shopts.threadsafe         = true;
+      76           0 :   shopts.autostart          = true;
+      77           0 :   shopts.queue_size         = 10;
+      78           0 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      79             : 
+      80           0 :   sh_gnss_ = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, gnss_topic_, &ProcMagDeclination::callbackGnss, this);
+      81             : 
+      82           0 :   is_initialized_ = true;
+      83           0 : }
+      84             : /*//}*/
+      85             : 
+      86             : /*//{ callbackGnss() */
+      87             : template <int n_measurements>
+      88           0 : void ProcMagDeclination<n_measurements>::callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg) {
+      89             : 
+      90           0 :   if (!is_initialized_) {
+      91           0 :     return;
+      92             :   }
+      93             : 
+      94           0 :   if (got_gnss_) {
+      95           0 :     return;
+      96             :   }
+      97             : 
+      98           0 :   if (!std::isfinite(msg->latitude)) {
+      99           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in GNSS variable \"msg->latitude\"!!!", Processor<n_measurements>::getPrintName().c_str());
+     100           0 :     return;
+     101             :   }
+     102             : 
+     103           0 :   if (!std::isfinite(msg->longitude)) {
+     104           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in GNSS variable \"msg->longitude\"!!!", Processor<n_measurements>::getPrintName().c_str());
+     105           0 :     return;
+     106             :   }
+     107             : 
+     108           0 :   std::scoped_lock lock(mtx_gnss_);
+     109           0 :   gnss_lat_ = msg->latitude;
+     110           0 :   gnss_lon_ = msg->longitude;
+     111           0 :   ROS_INFO("[%s]: First GNSS obtained: lat:%.2f lon:%.2f", Processor<n_measurements>::getPrintName().c_str(), gnss_lat_, gnss_lon_);
+     112           0 :   got_gnss_ = true;
+     113             : }
+     114             : /*//}*/
+     115             : 
+     116             : /*//{ process() */
+     117             : template <int n_measurements>
+     118           0 : std::tuple<bool, bool> ProcMagDeclination<n_measurements>::process(measurement_t& measurement) {
+     119             : 
+     120           0 :   if (!Processor<n_measurements>::enabled_) {
+     121           0 :     return {true, true};
+     122             :   }
+     123             : 
+     124           0 :   std::scoped_lock lock(mtx_gnss_);
+     125             : 
+     126           0 :   if (!got_gnss_) {
+     127           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: Missing GNSS data on topic: %s", Processor<n_measurements>::getPrintName().c_str(), gnss_topic_.c_str());
+     128           0 :     return {false, false};
+     129             :   }
+     130             : 
+     131           0 :   float mag_declination = GeoMagDeclination::getMagDeclinationRadians(gnss_lat_, gnss_lon_);
+     132             : 
+     133           0 :   measurement(0) -= mag_declination;
+     134             : 
+     135           0 :   return {true, true};
+     136             : }
+     137             : /*//}*/
+     138             : 
+     139             : /*//{ reset() */
+     140             : template <int n_measurements>
+     141           0 : void ProcMagDeclination<n_measurements>::reset() {
+     142           0 :   got_gnss_                  = false;
+     143           0 : }
+     144             : /*//}*/
+     145             : }  // namespace mrs_uav_state_estimators
+     146             : 
+     147             : #endif  // PROCESSORS_PROC_MAG_DECLINATION
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_mag_declination.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_mag_declination.h.gcov.overview.html new file mode 100644 index 0000000000..bfe3ee81a7 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_mag_declination.h.gcov.overview.html @@ -0,0 +1,57 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_mag_declination.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_state_estimators/include/mrs_uav_state_estimators/processors/proc_mag_declination.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_mag_declination.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..b7ae24169b850aef0f59fea98e932843a748108c GIT binary patch literal 677 zcmV;W0$TlvP)qCQt0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vprvaf#wyDW-YALzEmr18uh)jLV3)Zg{x{^c`#4p<*v(=lUZ8V z`{K8xt3qG;(SlB&u{(5cm+5CFNre$|=>+8BFLk&SN!EN%Y%5bTk#r8s)p$Xd7*#0Z zG#9oq^Tf%Tgi!_*N7I75e@p>fF@#=wsmO(ZX}8bFiD*cR7O>gWbI2POIhVts!M z4?bVv)&XN1>gf9dxUN!qi;;{X2a*CT7`v2M6P`d^am^U9<{L@}-z5+aQONr-C?j51 zwI;Pkq4ZJr)1dk+>)2LE)rRzbeZYV#>DSAn{HV)QSJPYEnCTGEtRVF524M1uZe&@N zNn_2QzAB{=%Wu1k7#RoJt@ILz(queon+ztPFajfVTa7QlgOP&cgac66WruQeq70O` zV|pU!`4jWT<6}}owos1-08Z=Z_qCuv9~pab#w?}3?~yh1#3Ng>I7b2V45cOfp=A2K z@lTTB6J~594tw%E^H>1KhVzi}7mx`o?D+B!<05Yt(}7}XEUSQ-ye7DXjSujSqJwAwLh9NPoOz=00000 LNkvXXu0mjfkB>Qw literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.func-sort-c.html new file mode 100644 index 0000000000..3fa47c138a --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.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_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-12-17 22:35:29Functions: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&)84
mrs_uav_state_estimators::ProcMedianFilter<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)140112
+
+
+ + + +
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..555ffb54a8 --- /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-12-17 22:35:29Functions: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>&)140112
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&)84
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..be543afe32 --- /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-12-17 22:35:29Functions: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          84 : 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          84 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph) {
+      41             : 
+      42             :   // | --------------------- load parameters -------------------- |
+      43          84 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      44             : 
+      45          84 :   ph->param_loader->loadParam("buffer_size", buffer_size_);
+      46          84 :   ph->param_loader->loadParam("max_diff", max_diff_);
+      47             : 
+      48          84 :   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          84 :   const double min_valid = std::numeric_limits<double>::lowest();
+      55          84 :   const double max_valid = std::numeric_limits<double>::max();
+      56             : 
+      57             :   // initialize median filter
+      58         168 :   for (int i = 0; i < n_measurements; i++) {
+      59          84 :     vec_mf_.push_back(mrs_lib::MedianFilter(buffer_size_, min_valid, max_valid, max_diff_));
+      60             :   }
+      61          84 : }
+      62             : /*//}*/
+      63             : 
+      64             : /*//{ process() */
+      65             : template <int n_measurements>
+      66      140112 : std::tuple<bool, bool> ProcMedianFilter<n_measurements>::process(measurement_t& measurement) {
+      67             : 
+      68      140112 :   if (!Processor<n_measurements>::enabled_) {
+      69           0 :     return {true, true};
+      70             :   }
+      71             : 
+      72      140112 :   bool ok_flag     = true;
+      73      140112 :   bool should_fuse = true;
+      74      280222 :   for (int i = 0; i < measurement.rows(); i++) {
+      75      140091 :     vec_mf_[i].add(measurement(i));
+      76      140106 :     if (vec_mf_[i].full()) {
+      77      131787 :       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        8315 :       ROS_WARN_THROTTLE(1.0, "[%s]: median filter not full yet", Processor<n_measurements>::getPrintName().c_str());
+      88        8316 :       ok_flag     = false;
+      89        8316 :       should_fuse = false;
+      90             :     }
+      91             :   }
+      92             : 
+      93      140091 :   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..d2dace3b55 --- /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-12-17 22:35:29Functions: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)>)86
mrs_uav_state_estimators::ProcSaturate<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)3879
mrs_uav_state_estimators::ProcSaturate<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)141919
+
+
+ + + +
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..969d718f8a --- /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-12-17 22:35:29Functions: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>&)141919
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)>)86
mrs_uav_state_estimators::ProcSaturate<2>::reset()0
mrs_uav_state_estimators::ProcSaturate<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)3879
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..0f557d51b4 --- /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-12-17 22:35:29Functions: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          90 : 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          90 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph), state_id_(state_id), fun_get_state_(fun_get_state) {
+      46             : 
+      47             :   // | --------------------- load parameters -------------------- |
+      48          90 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      49             : 
+      50          90 :   ph->param_loader->loadParam("start_enabled", this->start_enabled_);
+      51          90 :   this->enabled_ = this->start_enabled_;
+      52          90 :   ph->param_loader->loadParam("keep_enabled", keep_enabled_);
+      53          90 :   ph->param_loader->loadParam("min", saturate_min_);
+      54          90 :   ph->param_loader->loadParam("max", saturate_max_);
+      55          90 :   ph->param_loader->loadParam("limit", innovation_limit_);
+      56             : 
+      57          90 :   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          90 : }
+      62             : /*//}*/
+      63             : 
+      64             : /*//{ process() */
+      65             : template <int n_measurements>
+      66      145798 : std::tuple<bool, bool> ProcSaturate<n_measurements>::process(measurement_t& measurement) {
+      67             : 
+      68             :   // if no saturation is required, processing is successful
+      69      145798 :   if (!this->enabled_) {
+      70           0 :     return {true, true};
+      71             :   }
+      72             : 
+      73      145798 :   bool ok_flag     = true;
+      74      145798 :   bool should_fuse = true;
+      75      288819 :   for (int i = 0; i < measurement.rows(); i++) {
+      76      149665 :     const double state = fun_get_state_(state_id_, i);
+      77      149669 :     ROS_INFO_ONCE("[%s]: first state[%d][%d]: %.2f", Processor<n_measurements>::getNamespacedName().c_str(), state_id_, i, state);
+      78             : 
+      79      149669 :     if (measurement(i) > state + innovation_limit_ || measurement(i) < state - innovation_limit_) {
+      80        6649 :       return {true, true};  // do not even try to saturate, trigger innovation-based switch to other estimator
+      81             :     }
+      82             : 
+      83      143013 :     if (measurement(i) > state + saturate_max_) {
+      84         388 :       const double saturated = state + saturate_max_;
+      85         388 :       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         388 :       measurement(i) = saturated;
+      88         388 :       ok_flag        = false;
+      89         388 :       should_fuse    = true;
+      90      142630 :     } 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      139144 :   if (!this->keep_enabled_ && ok_flag) {
+     102           0 :     this->enabled_ = false;
+     103             :   }
+     104             : 
+     105      139144 :   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..c71b5aaaf4 --- /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-12-17 22:35:29Functions: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&)112
mrs_uav_state_estimators::ProcTfToWorld<2>::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)79273
mrs_uav_state_estimators::ProcTfToWorld<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)222583
+
+
+ + + +
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..d262bbaabb --- /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-12-17 22:35:29Functions: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>)79273
mrs_uav_state_estimators::ProcTfToWorld<2>::reset()0
mrs_uav_state_estimators::ProcTfToWorld<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)222583
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&)112
+
+
+ + + +
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..d56e4a3237 --- /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-12-17 22:35:29Functions: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         112 : 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         112 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph) {
+      51             : 
+      52         112 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      53             : 
+      54         112 :   ph->param_loader->loadParam("gnss_topic", gnss_topic_);
+      55             : 
+      56         112 :   gnss_topic_ = "/" + ch->uav_name + "/" + gnss_topic_;
+      57             : 
+      58         112 :   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         112 :   mrs_lib::SubscribeHandlerOptions shopts;
+      65         112 :   shopts.nh                 = nh;
+      66         112 :   shopts.node_name          = Processor<n_measurements>::getPrintName();
+      67         112 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      68         112 :   shopts.threadsafe         = true;
+      69         112 :   shopts.autostart          = true;
+      70         112 :   shopts.queue_size         = 10;
+      71         112 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      72             : 
+      73         112 :   sh_gnss_ = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, gnss_topic_, &ProcTfToWorld::callbackGnss, this);
+      74             : 
+      75         112 :   is_initialized_ = true;
+      76         112 : }
+      77             : /*//}*/
+      78             : 
+      79             : /*//{ callbackGnss() */
+      80             : template <int n_measurements>
+      81       79273 : void ProcTfToWorld<n_measurements>::callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg) {
+      82             : 
+      83       79273 :   if (!is_initialized_) {
+      84       79113 :     return;
+      85             :   }
+      86             : 
+      87       79273 :   if (got_gnss_) {
+      88       79113 :     return;
+      89             :   }
+      90             : 
+      91         111 :   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         112 :   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         224 :   std::scoped_lock lock(mtx_gnss_);
+     102         112 :   mrs_lib::UTM(msg->latitude, msg->longitude, &gnss_x_, &gnss_y_);
+     103         112 :   ROS_INFO("[%s]: First GNSS obtained: %.2f %.2f", Processor<n_measurements>::getPrintName().c_str(), gnss_x_, gnss_y_);
+     104         112 :   got_gnss_ = true;
+     105             : }
+     106             : /*//}*/
+     107             : 
+     108             : /*//{ process() */
+     109             : template <int n_measurements>
+     110      222583 : std::tuple<bool, bool> ProcTfToWorld<n_measurements>::process(measurement_t& measurement) {
+     111             : 
+     112      222583 :   if (!Processor<n_measurements>::enabled_) {
+     113           0 :     return {true, true};
+     114             :   }
+     115             : 
+     116      445111 :   std::scoped_lock lock(mtx_gnss_);
+     117             : 
+     118      222542 :   if (!got_gnss_) {
+     119        7897 :     ROS_WARN_THROTTLE(1.0, "[%s]: Missing GNSS data on topic: %s", Processor<n_measurements>::getPrintName().c_str(), gnss_topic_.c_str());
+     120        7897 :     return {false, false};
+     121             :   }
+     122             : 
+     123      214606 :   if (!is_gnss_offset_calculated_) {
+     124             : 
+     125         111 :     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         112 :     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         112 :     gnss_x_                    = (gnss_x_ - measurement(0)) - Processor<n_measurements>::ch_->world_origin.x;
+     130         112 :     gnss_y_                    = (gnss_y_ - measurement(1)) - Processor<n_measurements>::ch_->world_origin.y;
+     131         112 :     is_gnss_offset_calculated_ = true;
+     132         112 :     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      214619 :   measurement(0) += gnss_x_;
+     136      214555 :   measurement(1) += gnss_y_;
+     137             : 
+     138      214643 :   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      214710 :   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-12-17 22:35:29Functions: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&)116
mrs_uav_state_estimators::Processor<2>::getNamespacedName[abi:cxx11]() const119
mrs_uav_state_estimators::Processor<1>::getPrintName[abi:cxx11]() const173
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&)254
mrs_uav_state_estimators::Processor<1>::getNamespacedName[abi:cxx11]() const338
mrs_uav_state_estimators::Processor<2>::getPrintName[abi:cxx11]() const585
+
+
+ + + +
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..be159cfeab --- /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-12-17 22:35:29Functions: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&)254
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&)116
mrs_uav_state_estimators::Processor<1>::getPrintName[abi:cxx11]() const173
mrs_uav_state_estimators::Processor<1>::getNamespacedName[abi:cxx11]() const338
mrs_uav_state_estimators::Processor<2>::getPrintName[abi:cxx11]() const585
mrs_uav_state_estimators::Processor<2>::getNamespacedName[abi:cxx11]() const119
+
+
+ + + +
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..615b6b3aad --- /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-12-17 22:35:29Functions: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         370 :   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         370 :       : correction_name_(correction_name), name_(name), ch_(ch), ph_(ph) {
+      30         370 :   }  // 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         457 : std::string Processor<n_measurements>::getNamespacedName() const {
+      52         457 :   return correction_name_ + "/" + name_;
+      53             : }
+      54             : /*//}*/
+      55             : 
+      56             : /*//{ getPrintName() */
+      57             : template <int n_measurements>
+      58         758 : std::string Processor<n_measurements>::getPrintName() const {
+      59         758 :   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:8212366.7 %
Date:2024-12-17 22:35:29Functions: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()80
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&)80
mrs_uav_state_estimators::garmin_agl::GarminAgl::start()187
mrs_uav_state_estimators::garmin_agl::GarminAgl::timerCheckHealth(ros::TimerEvent const&)1340
mrs_uav_state_estimators::garmin_agl::GarminAgl::timerUpdate(ros::TimerEvent const&)129683
mrs_uav_state_estimators::garmin_agl::GarminAgl::getUavAglHeight() const131678
+
+
+ + + +
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..6139194d0e --- /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:8212366.7 %
Date:2024-12-17 22:35:29Functions:61060.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()80
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&)80
mrs_uav_state_estimators::garmin_agl::GarminAgl::isConverged()0
mrs_uav_state_estimators::garmin_agl::GarminAgl::timerUpdate(ros::TimerEvent const&)129683
mrs_uav_state_estimators::garmin_agl::GarminAgl::timerCheckHealth(ros::TimerEvent const&)1340
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()187
mrs_uav_state_estimators::garmin_agl::GarminAgl::getUavAglHeight() const131678
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..0a3f4d1ca0 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.html @@ -0,0 +1,353 @@ + + + + + + + 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:8212366.7 %
Date:2024-12-17 22:35:29Functions: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          80 : void GarminAgl::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      15             : 
+      16          80 :   ch_ = ch;
+      17          80 :   ph_ = ph;
+      18          80 :   nh_ = nh;
+      19             : 
+      20          80 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      21             : 
+      22             :   // | --------------------- load parameters -------------------- |
+      23             : 
+      24          80 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getName() + "/");
+      25             : 
+      26          80 :   if (is_core_plugin_) {
+      27             : 
+      28          80 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml");
+      29          80 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml");
+      30             :   }
+      31             : 
+      32          80 :   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          80 :   _update_timer_rate_       = 100;                                                                                          // TODO: parametrize
+      39          80 :   timer_update_             = nh.createTimer(ros::Rate(_update_timer_rate_), &GarminAgl::timerUpdate, this, false, false);  // not running after init
+      40          80 :   _check_health_timer_rate_ = 1;                                                                                            // TODO: parametrize
+      41          80 :   timer_check_health_       = nh.createTimer(ros::Rate(_check_health_timer_rate_), &GarminAgl::timerCheckHealth, this);
+      42             : 
+      43             :   // | --------------- subscribers initialization --------------- |
+      44         160 :   mrs_lib::SubscribeHandlerOptions shopts;
+      45          80 :   shopts.nh                 = nh;
+      46          80 :   shopts.node_name          = getPrintName();
+      47          80 :   shopts.no_message_timeout = ros::Duration(0.5);
+      48          80 :   shopts.threadsafe         = true;
+      49          80 :   shopts.autostart          = true;
+      50          80 :   shopts.queue_size         = 10;
+      51          80 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      52             : 
+      53             :   // | ---------------- publishers initialization --------------- |
+      54          80 :   ph_agl_height_ = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh, Support::toSnakeCase(getName()) + "/agl_height", 10);
+      55          80 :   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          80 :   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          80 :   est_agl_garmin_ = std::make_unique<AltGeneric>(est_agl_name_, frame_id_, getName(), is_core_plugin_);
+      65          80 :   est_agl_garmin_->initialize(nh, ch_, ph_);
+      66             : 
+      67          80 :   max_flight_z_ = est_agl_garmin_->getMaxFlightZ();
+      68             : 
+      69             :   // | ------------------ initialize published messages ------------------ |
+      70          80 :   agl_height_init_.header.frame_id     = ns_frame_id_;
+      71          80 :   agl_height_cov_init_.header.frame_id = ns_frame_id_;
+      72             : 
+      73             :   // | ------------------ finish initialization ----------------- |
+      74             : 
+      75          80 :   if (changeState(INITIALIZED_STATE)) {
+      76          80 :     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          80 : }
+      81             : /*//}*/
+      82             : 
+      83             : /*//{ start() */
+      84         187 : bool GarminAgl::start(void) {
+      85             : 
+      86             : 
+      87         187 :   if (isInState(READY_STATE)) {
+      88             : 
+      89             :     bool est_agl_garmin_start_successful;
+      90             : 
+      91         187 :     if (est_agl_garmin_->isStarted() || est_agl_garmin_->isRunning()) {
+      92           0 :       est_agl_garmin_start_successful = true;
+      93             :     } else {
+      94         187 :       est_agl_garmin_start_successful = est_agl_garmin_->start();
+      95             :     }
+      96             : 
+      97         187 :     if (est_agl_garmin_start_successful) {
+      98          80 :       timer_update_.start();
+      99          80 :       changeState(STARTED_STATE);
+     100          80 :       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         107 :   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      129683 : void GarminAgl::timerUpdate([[maybe_unused]] const ros::TimerEvent &event) {
+     147             : 
+     148      129683 :   if (!isInitialized()) {
+     149           0 :     return;
+     150             :   }
+     151             : 
+     152      129683 :   const ros::Time time_now = ros::Time::now();
+     153             : 
+     154      259366 :   mrs_msgs::Float64Stamped agl_height = agl_height_init_;
+     155      129683 :   agl_height.header.stamp             = time_now;
+     156      129683 :   agl_height.value                    = est_agl_garmin_->getState(POSITION);
+     157             : 
+     158      259366 :   mrs_msgs::Float64ArrayStamped agl_height_cov;
+     159      129683 :   agl_height_cov.header.stamp = time_now;
+     160             : 
+     161      129683 :   const int n_states = 2;  // TODO this should be defined somewhere else
+     162      129683 :   agl_height_cov.values.resize(n_states * n_states);
+     163      129683 :   agl_height_cov.values.at(n_states * POSITION + POSITION) = est_agl_garmin_->getCovariance(POSITION);
+     164      129683 :   agl_height_cov.values.at(n_states * VELOCITY + VELOCITY) = est_agl_garmin_->getCovariance(VELOCITY);
+     165             : 
+     166      129683 :   mrs_lib::set_mutexed(mtx_agl_height_, agl_height, agl_height_);
+     167      129683 :   mrs_lib::set_mutexed(mtx_agl_height_cov_, agl_height_cov, agl_height_cov_);
+     168             : 
+     169      129683 :   publishAglHeight();
+     170      129683 :   publishCovariance();
+     171      129683 :   publishDiagnostics();
+     172             : }
+     173             : /*//}*/
+     174             : 
+     175             : /*//{ timerCheckHealth() */
+     176        1340 : void GarminAgl::timerCheckHealth([[maybe_unused]] const ros::TimerEvent &event) {
+     177             : 
+     178        1340 :   if (!isInitialized()) {
+     179           0 :     return;
+     180             :   }
+     181             : 
+     182        1340 :   switch (getCurrentSmState()) {
+     183             : 
+     184           0 :     case UNINITIALIZED_STATE: {
+     185           0 :       break;
+     186             :     }
+     187             : 
+     188          80 :     case INITIALIZED_STATE: {
+     189             : 
+     190          80 :       if (est_agl_garmin_->isInitialized()) {
+     191          80 :         changeState(READY_STATE);
+     192          80 :         ROS_INFO("[%s]: Estimator is ready to start", getPrintName().c_str());
+     193             :       } else {
+     194           0 :         ROS_INFO("[%s]: %s subestimators to be ready", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     195           0 :         return;
+     196             :       }
+     197          80 :       break;
+     198             :     }
+     199             : 
+     200           1 :     case READY_STATE: {
+     201           1 :       break;
+     202             :     }
+     203             : 
+     204          80 :     case STARTED_STATE: {
+     205             : 
+     206          80 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s for convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     207             : 
+     208          80 :       if (est_agl_garmin_->isError()) {
+     209           0 :         changeState(ERROR_STATE);
+     210             :       }
+     211             : 
+     212          80 :       if (est_agl_garmin_->isRunning()) {
+     213          80 :         ROS_INFO_THROTTLE(1.0, "[%s]: Subestimators converged", getPrintName().c_str());
+     214          80 :         changeState(RUNNING_STATE);
+     215             :       } else {
+     216           0 :         return;
+     217             :       }
+     218          80 :       break;
+     219             :     }
+     220             : 
+     221        1179 :     case RUNNING_STATE: {
+     222        1179 :       if (est_agl_garmin_->isError()) {
+     223           0 :         changeState(ERROR_STATE);
+     224             :       }
+     225        1179 :       break;
+     226             :     }
+     227             : 
+     228           0 :     case STOPPED_STATE: {
+     229           0 :       break;
+     230             :     }
+     231             : 
+     232           0 :     case ERROR_STATE: {
+     233           0 :       if (est_agl_garmin_->isReady()) {
+     234           0 :         changeState(READY_STATE);
+     235             :       }
+     236           0 :       break;
+     237             :     }
+     238             :   }
+     239             : }
+     240             : /*//}*/
+     241             : 
+     242             : /*//{ isConverged() */
+     243           0 : bool GarminAgl::isConverged() {
+     244             : 
+     245             :   // TODO: check convergence by rate of change of determinant
+     246             :   // most likely not used in top-level estimator
+     247             : 
+     248           0 :   return true;
+     249             : }
+     250             : /*//}*/
+     251             : 
+     252             : /*//{ getUavAglHeight() */
+     253      131678 : mrs_msgs::Float64Stamped GarminAgl::getUavAglHeight() const {
+     254      131678 :   return mrs_lib::get_mutexed(mtx_agl_height_, agl_height_);
+     255             : }
+     256             : /*//}*/
+     257             : 
+     258             : /*//{ getHeightCovariance() */
+     259           0 : std::vector<double> GarminAgl::getHeightCovariance() const {
+     260           0 :   return mrs_lib::get_mutexed(mtx_agl_height_cov_, agl_height_cov_.values);
+     261             : }
+     262             : /*//}*/
+     263             : 
+     264             : }  // namespace garmin_agl
+     265             : 
+     266             : }  // namespace mrs_uav_state_estimators
+     267             : 
+     268             : #include <pluginlib/class_list_macros.h>
+     269          80 : 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..41dd71bf32 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.overview.html @@ -0,0 +1,88 @@ + + + + + + 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 + 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..13cf2cc71f10656ff9e17ab5dbe931e54b77fd46 GIT binary patch literal 1143 zcmV--1c>{IP)@P4ndLKHBTGISK+Vh~$>f`uTj=^;o_| zmpmgBAvPg#;ye8{GPo*?@#~s_>JH-rs%=(a3U>ylX8>qFcRsK zXzdi?w|$!{p-a3x4l-eNagUeSaEy5w7qdjEpOcXZu@-GcvP5K&5KF?Yk;+Jjvr#?Z z#|pwL9$5DMbfolCoyYa?pWUmz02ucsE&I`C2#j@%QBB4etukF@-5ijGr&1?Q^ejwOBYG3n$I(zcqI#wPv>i?+9_+2g zM1sf^e-X9p8s3IW%`#`CO3dq{^Usbb2Jr*4FF$CoL~;8k_`gkfRuF2c%c{p#ayf#MQT8G zd5XJtBH;xfMV8`r-4}ZHv|^fyS|a6Qysj%s4KF&)w7P&Tjs~nyRVj+3gn9Un7wQQ$e-N@T#c@|L@V$ke7f$ zxh5r6q7(G+4@8DcM1b!L5#U39Oe^!59BXDJ5^`%_h;H1U0PY$}c$=|0Bh}YLe5-f* zsYzU;d-Ux!!AB3^k;79$JhtE1GBs0xf9Py50=I_?Qz2|Od{8x+u_=;% zc719ab``B9yI##Uc+1Bi?$1>-Xg50Bpj)kti46_jf4D;~#cHD216K2m6q}JdK9y5$ zMY55Y@U&FJbeCpl4Mi_xsAeQhrA(I+IvPKVvhUz&LwUxo*EJ(4H0~fy^5X;J2N#)U z67rw1#!jUm>E*?UL|FJ(6$0MGzJqXNm)} z`t-kse=A`2$G&89+|}KzTiw9dzRX+wD@G*Xu4L0xSn#s)msG&K56`{>YR?ZjN;U0{ zP&_kI$=xa+>yi>YzZ+|2K~MUJ#R-Xy?nw-3*OOQxtRU0-`~&h>3Yf$Hn%V#W002ov JPDHLkV1lVC7m)w} 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..9089f73042 --- /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:8212366.7 %
Date:2024-12-17 22:35:29Functions: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 +
66.7%66.7%
+ 		66.7 %82 / 12360.0 %6 / 10
<unnamed>66.7 %82 / 12360.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..dad2092def --- /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:8212366.7 %
Date:2024-12-17 22:35:29Functions: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 +
66.7%66.7%
+ 		66.7 %82 / 12360.0 %6 / 10
<unnamed>66.7 %82 / 12360.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..be7c2bf5a0 --- /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:8212366.7 %
Date:2024-12-17 22:35:29Functions: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 +
66.7%66.7%
+ 		66.7 %82 / 12360.0 %6 / 10
<unnamed>66.7 %82 / 12360.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..d7b0b4cd98 --- /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:8212366.7 %
Date:2024-12-17 22:35:29Functions: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 +
66.7%66.7%
+ 		66.7 %82 / 12360.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..57baec97b3 --- /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:8212366.7 %
Date:2024-12-17 22:35:29Functions: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 +
66.7%66.7%
+ 		66.7 %82 / 12360.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..e983da0f03 --- /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:8212366.7 %
Date:2024-12-17 22:35:29Functions: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 +
66.7%66.7%
+ 		66.7 %82 / 12360.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..cb6457e8e8 --- /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:21339653.8 %
Date:2024-12-17 22:35:29Functions: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&)196
mrs_uav_state_estimators::AltGeneric::isConverged()196
mrs_uav_state_estimators::AltGeneric::callbackReconfigure(mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)196
mrs_uav_state_estimators::AltGeneric::setState(double const&, int const&)297
mrs_uav_state_estimators::AltGeneric::setInputCoeff(double const&)382
mrs_uav_state_estimators::AltGeneric::getNamespacedName[abi:cxx11]() const1262
mrs_uav_state_estimators::AltGeneric::getPrintName[abi:cxx11]() const1529
mrs_uav_state_estimators::AltGeneric::start()4600
mrs_uav_state_estimators::AltGeneric::getState(int const&, int const&) const141921
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) const141922
mrs_uav_state_estimators::AltGeneric::getInnovation(int const&) const210227
mrs_uav_state_estimators::AltGeneric::getCovarianceMatrix() const312474
mrs_uav_state_estimators::AltGeneric::getStates() const312721
mrs_uav_state_estimators::AltGeneric::setDt(double const&)312801
mrs_uav_state_estimators::AltGeneric::generateB()313376
mrs_uav_state_estimators::AltGeneric::generateA()313381
mrs_uav_state_estimators::AltGeneric::timerUpdate(ros::TimerEvent const&)357312
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) const491805
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&)491820
mrs_uav_state_estimators::AltGeneric::doCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)491836
mrs_uav_state_estimators::AltGeneric::getCovariance(int const&) const679817
mrs_uav_state_estimators::AltGeneric::getState(int const&) const1165175
+
+
+ + + +
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..3fe515269e --- /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:21339653.8 %
Date:2024-12-17 22:35:29Functions: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&)196
mrs_uav_state_estimators::AltGeneric::isConverged()196
mrs_uav_state_estimators::AltGeneric::timerUpdate(ros::TimerEvent const&)357312
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&)491820
mrs_uav_state_estimators::AltGeneric::doCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)491836
mrs_uav_state_estimators::AltGeneric::setInputCoeff(double const&)382
mrs_uav_state_estimators::AltGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::AltGeneric::callbackReconfigure(mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)196
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&)312801
mrs_uav_state_estimators::AltGeneric::start()4600
mrs_uav_state_estimators::AltGeneric::setState(double const&, int const&)297
mrs_uav_state_estimators::AltGeneric::setState(double const&, int const&, int const&)0
mrs_uav_state_estimators::AltGeneric::generateA()313381
mrs_uav_state_estimators::AltGeneric::generateB()313376
mrs_uav_state_estimators::AltGeneric::setStates(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)0
mrs_uav_state_estimators::AltGeneric::getPrintName[abi:cxx11]() const1529
mrs_uav_state_estimators::AltGeneric::getCovariance(int const&) const679817
mrs_uav_state_estimators::AltGeneric::getCovariance(int const&, int const&) const0
mrs_uav_state_estimators::AltGeneric::getInnovation(int const&) const210227
mrs_uav_state_estimators::AltGeneric::getInnovation(int const&, int const&) const0
mrs_uav_state_estimators::AltGeneric::getNamespacedName[abi:cxx11]() const1262
mrs_uav_state_estimators::AltGeneric::getCovarianceMatrix() const312474
mrs_uav_state_estimators::AltGeneric::getState(int const&) const1165175
mrs_uav_state_estimators::AltGeneric::getState(int const&, int const&) const141921
mrs_uav_state_estimators::AltGeneric::getStates() const312721
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) const491805
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) const141922
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..fdcfe68c87 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.html @@ -0,0 +1,840 @@ + + + + + + + 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:21339653.8 %
Date:2024-12-17 22:35:29Functions: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         196 : void AltGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      15             : 
+      16         196 :   ch_ = ch;
+      17         196 :   ph_ = ph;
+      18             : 
+      19         196 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      20             : 
+      21             :   // clang-format off
+      22         196 :   dt_ = 0.01;
+      23         196 :   input_coeff_ = 10;
+      24         196 :   default_input_coeff_ = 10;
+      25             : 
+      26         196 :   generateA();
+      27         196 :   generateB();
+      28             : 
+      29         196 :   H_ <<
+      30         196 :     1, 0, 0;
+      31             : 
+      32         196 :   innovation_ << 
+      33             :     0;
+      34             : 
+      35             :   // clang-format on
+      36             : 
+      37             :   // | --------------- initialize parameter loader -------------- |
+      38             : 
+      39         196 :   if (is_core_plugin_) {
+      40             : 
+      41         196 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      42         196 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      43             :   }
+      44             : 
+      45         196 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      46             : 
+      47             :   // | --------------------- load parameters -------------------- |
+      48             : 
+      49         196 :   ph->param_loader->loadParam("max_flight_z", max_flight_z_);
+      50         196 :   ph->param_loader->loadParam("innovation/limit", pos_innovation_limit_);
+      51         196 :   ph->param_loader->loadParam("innovation/action", exc_innovation_action_name_);
+      52         196 :   exc_innovation_action_ = map_exc_inno_action.at(exc_innovation_action_name_);
+      53         196 :   ph->param_loader->loadParam("repredictor/enabled", is_repredictor_enabled_);
+      54         196 :   if (is_repredictor_enabled_) {
+      55           0 :     ph->param_loader->loadParam("repredictor/buffer_size", rep_buffer_size_);
+      56             :   }
+      57             : 
+      58             :   // | --------------- corrections initialization --------------- |
+      59         196 :   ph->param_loader->loadParam("corrections", correction_names_);
+      60             : 
+      61         478 :   for (auto corr_name : correction_names_) {
+      62         282 :     corrections_.push_back(std::make_shared<Correction<alt_generic::n_measurements>>(
+      63      142486 :         nh, getNamespacedName(), corr_name, ns_frame_id_, EstimatorType_t::ALTITUDE, ch_, ph_, [this](int a, int b) { return this->getState(a, b); },
+      64      491805 :         [this](const Correction<alt_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t state) {
+      65      491805 :           return this->doCorrection(meas, R, state);
+      66             :         }));
+      67             :   }
+      68             : 
+      69         196 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      70             : 
+      71             :   // | ----------- initialize process noise covariance ---------- |
+      72         196 :   Q_ = Q_t::Zero();
+      73             :   double tmp_noise;
+      74         196 :   ph->param_loader->loadParam("process_noise/pos", tmp_noise);
+      75         196 :   Q_(POSITION, POSITION) = tmp_noise;
+      76         196 :   ph->param_loader->loadParam("process_noise/vel", tmp_noise);
+      77         196 :   Q_(VELOCITY, VELOCITY) = tmp_noise;
+      78         196 :   ph->param_loader->loadParam("process_noise/acc", tmp_noise);
+      79         196 :   Q_(ACCELERATION, ACCELERATION) = tmp_noise;
+      80             : 
+      81             :   // | ------- check if all parameters loaded successfully ------ |
+      82         196 :   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         196 :       std::make_unique<drmgr_t>(ros::NodeHandle("~/" + getNamespacedName()), true, getPrintName(), boost::bind(&AltGeneric::callbackReconfigure, this, _1, _2));
+      90         196 :   drmgr_->config.pos = Q_(POSITION, POSITION);
+      91         196 :   drmgr_->config.vel = Q_(VELOCITY, VELOCITY);
+      92         196 :   drmgr_->config.acc = Q_(ACCELERATION, ACCELERATION);
+      93         196 :   drmgr_->update_config(drmgr_->config);
+      94             : 
+      95             :   // | --------------- Kalman filter intialization -------------- |
+      96         196 :   const x_t        x0 = x_t::Zero();
+      97         196 :   const P_t        P0 = 1e3 * P_t::Identity();
+      98         196 :   const statecov_t sc0({x0, P0});
+      99         196 :   sc_ = sc0;
+     100             : 
+     101         196 :   lkf_ = std::make_shared<lkf_t>(A_, B_, H_);
+     102         196 :   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         196 :   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         392 :   mrs_lib::SubscribeHandlerOptions shopts;
+     120         196 :   shopts.nh                 = nh;
+     121         196 :   shopts.node_name          = getPrintName();
+     122         196 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     123         196 :   shopts.threadsafe         = true;
+     124         196 :   shopts.autostart          = true;
+     125         196 :   shopts.queue_size         = 10;
+     126         196 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     127             : 
+     128         196 :   sh_control_input_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput>(shopts, "control_input_in");
+     129             : 
+     130             :   // | ---------------- publishers initialization --------------- |
+     131         196 :   if (ch_->debug_topics.input) {
+     132         196 :     ph_input_ = mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped>(nh, getNamespacedName() + "/input", 10);
+     133             :   }
+     134         196 :   if (ch_->debug_topics.output) {
+     135         196 :     ph_output_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput>(nh, getNamespacedName() + "/output", 10);
+     136             :   }
+     137         196 :   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         196 :   if (changeState(INITIALIZED_STATE)) {
+     144         196 :     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         196 : }
+     149             : /*//}*/
+     150             : 
+     151             : /*//{ start() */
+     152        4600 : bool AltGeneric::start(void) {
+     153             : 
+     154        4600 :   if (isInState(READY_STATE)) {
+     155             :     /* timer_update_.start(); */
+     156         196 :     changeState(STARTED_STATE);
+     157         196 :     return true;
+     158             : 
+     159             :   } else {
+     160        4404 :     ROS_WARN_THROTTLE(1.0, "[%s]: Estimator must be in READY_STATE to start it", getPrintName().c_str());
+     161        4404 :     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 :   changeState(INITIALIZED_STATE);
+     205           0 :   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
+     206             : 
+     207           0 :   return true;
+     208             : }
+     209             : /*//}*/
+     210             : 
+     211             : /* timerUpdate() //{*/
+     212      357312 : void AltGeneric::timerUpdate(const ros::TimerEvent &event) {
+     213             : 
+     214             : 
+     215      357312 :   if (!isInitialized()) {
+     216       44523 :     return;
+     217             :   }
+     218             : 
+     219      357255 :   switch (getCurrentSmState()) {
+     220             : 
+     221           0 :     case UNINITIALIZED_STATE: {
+     222           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s initialization", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     223           0 :       break;
+     224             :     }
+     225             : 
+     226        6577 :     case READY_STATE: {
+     227        6577 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s estimator start", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     228        6577 :       break;
+     229             :     }
+     230             : 
+     231        8849 :     case INITIALIZED_STATE: {
+     232             :       // initialize the estimator with current corrections
+     233        9147 :       for (auto correction : corrections_) {
+     234        8951 :         auto res = correction->getProcessedCorrection();
+     235        8943 :         if (res) {
+     236         297 :           auto measurement_stamped = res.value();
+     237         297 :           setState(measurement_stamped.value(0), correction->getStateId());
+     238         297 :           ROS_INFO("[%s]: Setting initial state to: %.2f", getPrintName().c_str(), measurement_stamped.value(0));
+     239             :         } else {
+     240        8650 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(),
+     241             :                             correction->getNamespacedName().c_str());
+     242        8647 :           return;
+     243             :         }
+     244             :       }
+     245         196 :       changeState(READY_STATE);
+     246         196 :       ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     247         196 :       break;
+     248             :     }
+     249             : 
+     250         196 :     case STARTED_STATE: {
+     251         196 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s for convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     252             : 
+     253         196 :       if (isConverged()) {
+     254         196 :         ROS_INFO_THROTTLE(1.0, "[%s]: LKF converged", getPrintName().c_str());
+     255         196 :         changeState(RUNNING_STATE);
+     256             :       }
+     257         196 :       break;
+     258             :     }
+     259             : 
+     260      341677 :     case RUNNING_STATE: {
+     261      825834 :       for (auto correction : corrections_) {
+     262      484117 :         if (!correction->isHealthy()) {
+     263           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     264           0 :           changeState(ERROR_STATE);
+     265             :         }
+     266             :       }
+     267      341690 :       break;
+     268             :     }
+     269             : 
+     270           0 :     case STOPPED_STATE: {
+     271           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is stopped", getPrintName().c_str());
+     272           0 :       break;
+     273             :     }
+     274             : 
+     275           0 :     case ERROR_STATE: {
+     276           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is in ERROR state", getPrintName().c_str());
+     277           0 :       ros::Time t_now = ros::Time::now();
+     278           0 :       if (is_error_state_first_time_) {
+     279           0 :         prev_time_in_error_state_  = t_now;
+     280           0 :         is_error_state_first_time_ = false;
+     281           0 :         error_state_duration_      = ros::Duration(0.0);
+     282             :       }
+     283           0 :       error_state_duration_ += t_now - prev_time_in_error_state_;
+     284             : 
+     285             : 
+     286             :       // check if all corrections are healthy now
+     287           0 :       bool all_corrections_healthy = true;
+     288           0 :       for (auto correction : corrections_) {
+     289           0 :         if (!correction->isHealthy()) {
+     290           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     291           0 :           all_corrections_healthy = false;
+     292             :         }
+     293             :       }
+     294             : 
+     295           0 :       if (all_corrections_healthy && innovation_ok_) {
+     296             :         // initialize the estimator again if corrections become healthy
+     297           0 :         if (error_state_duration_.toSec() > 5.0) {
+     298           0 :           ROS_INFO("[%s]: corrections healthy for %.2f s", getPrintName().c_str(), error_state_duration_.toSec());
+     299           0 :           changeState(INITIALIZED_STATE);
+     300           0 :           is_error_state_first_time_ = true;
+     301           0 :         }
+     302             :       } else {
+     303           0 :         is_error_state_first_time_ = true;
+     304             :       }
+     305             : 
+     306           0 :       prev_time_in_error_state_ = t_now;
+     307             : 
+     308           0 :       break;
+     309             :     }
+     310             :   }
+     311             : 
+     312      348674 :   if (sh_control_input_.newMsg()) {
+     313      176622 :     is_input_ready_ = true;
+     314             :   }
+     315             : 
+     316             :   // check age of input
+     317      348644 :   if (is_input_ready_ && (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {
+     318          27 :     ROS_WARN_THROTTLE(1.0, "[%s]: input too old (%.4f), using zero input instead", getPrintName().c_str(),
+     319             :                       (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec());
+     320          27 :     is_input_ready_ = false;
+     321             :   }
+     322             : 
+     323      348599 :   if (!isRunning() && !isStarted()) {
+     324        6760 :     return;
+     325             :   }
+     326             : 
+     327      341811 :   if (first_iter_) {
+     328         196 :     first_iter_ = false;
+     329         196 :     return;
+     330             :   }
+     331             : 
+     332      341615 :   double dt = (event.current_real - event.last_real).toSec();
+     333      341643 :   if (dt <= 0.0 || dt > 1.0) {
+     334       28918 :     return;
+     335             :   }
+     336             : 
+     337      312725 :   if (!is_repredictor_enabled_) {  // repredictor calculates dt on its own
+     338      312733 :     setDt(dt);
+     339             :   }
+     340             : 
+     341             :   // prediction step
+     342      312660 :   u_t       u;
+     343      312610 :   ros::Time input_stamp;
+     344      312583 :   if (is_input_ready_) {
+     345      200378 :     mrs_msgs::EstimatorInputConstPtr msg            = sh_control_input_.getMsg();
+     346      200426 :     const tf2::Vector3               des_acc_global = getAccGlobal(msg, 0);  // we don't care about heading
+     347      200237 :     input_stamp                                     = msg->header.stamp;
+     348      199944 :     if (input_coeff_ != default_input_coeff_){
+     349         158 :       setInputCoeff(default_input_coeff_);
+     350             :     }
+     351      199941 :     u(0) = des_acc_global.getZ();
+     352             :   } else {
+     353      112140 :     ROS_DEBUG_THROTTLE(1.0, "[%s]: not receiving control input, estimation suboptimal, potentially unstable", getPrintName().c_str());
+     354      112242 :     input_stamp = ros::Time::now();
+     355      112265 :     if (input_coeff_ != 0){
+     356         223 :       setInputCoeff(0);
+     357             :     }
+     358      112265 :     u = u_t::Zero();
+     359             :   }
+     360             : 
+     361             :   // go through available corrections and apply them
+     362             :   /* for (auto correction : corrections_) { */
+     363             :   /*   auto res = correction->getProcessedCorrection(); */
+     364             :   /*   if (res) { */
+     365             :   /*     auto measurement_stamped = res.value(); */
+     366             :   /*     doCorrection(measurement_stamped.value, correction->getR(), correction->getStateId(), measurement_stamped.stamp); */
+     367             :   /*   } */
+     368             :   /* } */
+     369             : 
+     370      311051 :   statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+     371      312334 :   Q_t        Q  = mrs_lib::get_mutexed(mtx_Q_, Q_);
+     372             : 
+     373             :   try {
+     374             :     // Apply the prediction step
+     375      623700 :     std::scoped_lock lock(mutex_lkf_);
+     376      311779 :     if (is_repredictor_enabled_) {
+     377           0 :       lkf_rep_->addInputChangeWithNoise(u, Q, input_stamp, models_[0]);
+     378           0 :       sc = lkf_rep_->predictTo(ros::Time::now());
+     379             :     } else {
+     380      311779 :       sc = lkf_->predict(sc, u, Q, dt_);
+     381             :     }
+     382             :   }
+     383           0 :   catch (const std::exception &e) {
+     384             :     // In case of error, alert the user
+     385           0 :     ROS_ERROR("[%s]: LKF prediction failed: %s", getPrintName().c_str(), e.what());
+     386             :   }
+     387             : 
+     388      310275 :   mrs_lib::set_mutexed(mutex_sc_, sc, sc_);
+     389             : 
+     390             :   // publishing
+     391      311482 :   publishInput(u, input_stamp);
+     392      312785 :   publishOutput();
+     393      312816 :   publishDiagnostics();
+     394             : }
+     395             : /*//}*/
+     396             : 
+     397             : /*//{ timerCheckHealth() */
+     398           0 : void AltGeneric::timerCheckHealth([[maybe_unused]] const ros::TimerEvent &event) {
+     399             : 
+     400           0 :   if (!isInitialized()) {
+     401           0 :     return;
+     402             :   }
+     403             : 
+     404           0 :   switch (getCurrentSmState()) {
+     405             : 
+     406           0 :     case UNINITIALIZED_STATE: {
+     407           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s initialization", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     408           0 :       break;
+     409             :     }
+     410             : 
+     411           0 :     case READY_STATE: {
+     412           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s estimator start", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     413           0 :       break;
+     414             :     }
+     415             : 
+     416           0 :     case INITIALIZED_STATE: {
+     417             : 
+     418             :       // initialize the estimator with current corrections
+     419           0 :       for (auto correction : corrections_) {
+     420           0 :         auto res = correction->getProcessedCorrection();
+     421           0 :         if (res) {
+     422           0 :           auto measurement_stamped = res.value();
+     423           0 :           setState(measurement_stamped.value(0), correction->getStateId());
+     424           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: Setting initial state to: %.2f", getPrintName().c_str(), measurement_stamped.value(0));
+     425             :         } else {
+     426           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(),
+     427             :                             correction->getNamespacedName().c_str());
+     428           0 :           return;
+     429             :         }
+     430             :       }
+     431           0 :       changeState(READY_STATE);
+     432           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     433           0 :       break;
+     434             :     }
+     435             : 
+     436           0 :     case STARTED_STATE: {
+     437           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s for convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     438             : 
+     439           0 :       if (isConverged()) {
+     440           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: LKF converged", getPrintName().c_str());
+     441           0 :         changeState(RUNNING_STATE);
+     442             :       }
+     443           0 :       break;
+     444             :     }
+     445             : 
+     446           0 :     case RUNNING_STATE: {
+     447           0 :       for (auto correction : corrections_) {
+     448           0 :         if (!correction->isHealthy()) {
+     449           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     450           0 :           changeState(ERROR_STATE);
+     451             :         }
+     452             :       }
+     453           0 :       break;
+     454             :     }
+     455             : 
+     456           0 :     case STOPPED_STATE: {
+     457           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is stopped", getPrintName().c_str());
+     458           0 :       break;
+     459             :     }
+     460             : 
+     461           0 :     case ERROR_STATE: {
+     462           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is in ERROR state", getPrintName().c_str());
+     463           0 :       bool all_corrections_healthy = true;
+     464           0 :       for (auto correction : corrections_) {
+     465           0 :         if (!correction->isHealthy()) {
+     466           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     467           0 :           all_corrections_healthy = false;
+     468             :         }
+     469             :       }
+     470             :       // initialize the estimator again if corrections become healthy
+     471           0 :       if (all_corrections_healthy) {
+     472           0 :         changeState(INITIALIZED_STATE);
+     473             :       }
+     474           0 :       break;
+     475             :     }
+     476             :   }
+     477             : 
+     478           0 :   if (sh_control_input_.newMsg()) {
+     479           0 :     is_input_ready_ = true;
+     480             :   }
+     481             : 
+     482             :   // check age of input
+     483           0 :   if (is_input_ready_ && (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {
+     484           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: input too old (%.4f), using zero input instead", getPrintName().c_str(),
+     485             :                       (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec());
+     486           0 :     is_input_ready_ = false;
+     487             :   }
+     488             : }
+     489             : /*//}*/
+     490             : 
+     491             : /*//{ doCorrection() */
+     492      491836 : void AltGeneric::doCorrection(const Correction<alt_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t &state_id) {
+     493      491836 :   doCorrection(meas.value, R, state_id, meas.stamp);
+     494      491659 : }
+     495             : /*//}*/
+     496             : 
+     497             : /*//{ doCorrection() */
+     498      491820 : void AltGeneric::doCorrection(const z_t &z, const double R, const StateId_t &H_idx, const ros::Time &meas_stamp) {
+     499             : 
+     500      491820 :   if (!isInitialized()) {
+     501        4589 :     return;
+     502             :   }
+     503             : 
+     504             :   // we do not want to perform corrections until the estimator is initialized
+     505      491695 :   if (!(isInState(SMStates_t::READY_STATE) || isInState(SMStates_t::RUNNING_STATE) || isInState(SMStates_t::STARTED_STATE))) {
+     506        4588 :     return;
+     507             :   }
+     508             : 
+     509             :   // for position state check the innovation
+     510      486959 :   if (H_idx == POSITION) {
+     511      262812 :     std::scoped_lock lock(mtx_innovation_);
+     512             : 
+     513      262840 :     is_mitigating_jump_ = false;
+     514      262892 :     innovation_(0)      = z(0) - getState(POSITION);
+     515             : 
+     516      262897 :     if (fabs(innovation_(0)) > pos_innovation_limit_) {
+     517           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: innovation too large - [%.2f] lim: %.2f", getPrintName().c_str(), innovation_(0), pos_innovation_limit_);
+     518           0 :       innovation_ok_ = false;
+     519           0 :       switch (exc_innovation_action_) {
+     520           0 :         case ExcInnoAction_t::ELAND: {
+     521           0 :           ROS_WARN_THROTTLE(1.0, "[%s]: large innovation should trigger eland in control manager", getPrintName().c_str());
+     522           0 :           changeState(ERROR_STATE);
+     523           0 :           break;
+     524             :         }
+     525           0 :         case ExcInnoAction_t::SWITCH: {
+     526           0 :           ROS_WARN_THROTTLE(1.0, "[%s]: innovation should trigger estimator switch but no eland", getPrintName().c_str());
+     527           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?
+     528           0 :           changeState(ERROR_STATE);
+     529           0 :           break;
+     530             :         }
+     531           0 :         case ExcInnoAction_t::MITIGATE: {
+     532           0 :           ROS_WARN_THROTTLE(1.0, "[%s]: large innovation should trigger estimate jump mitigation", getPrintName().c_str());
+     533           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?
+     534           0 :           is_mitigating_jump_ = true;
+     535           0 :           setState(z(0), POSITION);
+     536           0 :           break;
+     537             :         }
+     538           0 :         case ExcInnoAction_t::NONE: {
+     539           0 :           ROS_WARN_THROTTLE(1.0, "[%s]: large innovation ignored", getPrintName().c_str());
+     540           0 :           break;
+     541             :         }
+     542             :       }
+     543             :     }
+     544      262902 :     innovation_ok_ = true;
+     545             :   }
+     546             : 
+     547      487038 :   statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+     548             :   try {
+     549             :     // Apply the correction step
+     550             :     {
+     551      972528 :       std::scoped_lock lock(mutex_lkf_);
+     552      487204 :       H_        = H_t::Zero();
+     553      487174 :       H_(H_idx) = 1;
+     554      486673 :       lkf_->H   = H_;
+     555      485462 :       if (is_repredictor_enabled_) {
+     556             : 
+     557           0 :         lkf_rep_->addMeasurement(z, R_t::Ones() * R, meas_stamp, models_[H_idx]);
+     558             :       } else {
+     559      485462 :         sc = lkf_->correct(sc, z, R_t::Ones() * R);
+     560             :       }
+     561             :     }
+     562             :   }
+     563           0 :   catch (const std::exception &e) {
+     564             :     // In case of error, alert the user
+     565           0 :     ROS_ERROR("[%s]: LKF correction failed: %s", getPrintName().c_str(), e.what());
+     566             :   }
+     567             : 
+     568      483372 :   mrs_lib::set_mutexed(mutex_sc_, sc, sc_);
+     569             : }  // namespace mrs_uav_state_estimators
+     570             : /*//}*/
+     571             : 
+     572             : /*//{ isConverged() */
+     573         196 : bool AltGeneric::isConverged() {
+     574             : 
+     575             :   // TODO: check convergence by rate of change of determinant
+     576             : 
+     577         196 :   return true;
+     578             : }
+     579             : /*//}*/
+     580             : 
+     581             : /*//{ getState() */
+     582      141921 : double AltGeneric::getState(const int &state_id_in, const int &axis_in) const {
+     583      141921 :   return getState(stateIdToIndex(state_id_in, axis_in));
+     584             : }
+     585             : 
+     586     1165175 : double AltGeneric::getState(const int &state_idx_in) const {
+     587     1165175 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).x(state_idx_in);
+     588             : }
+     589             : /*//}*/
+     590             : 
+     591             : /*//{ setState() */
+     592           0 : void AltGeneric::setState(const double &state_in, const int &state_id_in, const int &axis_in) {
+     593           0 :   setState(state_in, stateIdToIndex(state_id_in, axis_in));
+     594           0 : }
+     595             : 
+     596         297 : void AltGeneric::setState(const double &state_in, const int &state_idx_in) {
+     597         297 :   mrs_lib::set_mutexed(mutex_sc_, state_in, sc_.x(state_idx_in));
+     598         297 : }
+     599             : /*//}*/
+     600             : 
+     601             : /*//{ getStates() */
+     602      312721 : AltGeneric::states_t AltGeneric::getStates(void) const {
+     603      624654 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).x;
+     604             : }
+     605             : /*//}*/
+     606             : 
+     607             : /*//{ setStates() */
+     608           0 : void AltGeneric::setStates(const states_t &states_in) {
+     609           0 :   mrs_lib::set_mutexed(mutex_sc_, states_in, sc_.x);
+     610           0 : }
+     611             : /*//}*/
+     612             : 
+     613             : /*//{ getCovariance() */
+     614           0 : double AltGeneric::getCovariance(const int &state_id_in, const int &axis_in) const {
+     615           0 :   return getCovariance(stateIdToIndex(state_id_in, axis_in));
+     616             : }
+     617             : 
+     618      679817 : double AltGeneric::getCovariance(const int &state_idx_in) const {
+     619      679817 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).P(state_idx_in, state_idx_in);
+     620             : }
+     621             : /*//}*/
+     622             : 
+     623             : /*//{ getCovarianceMatrix() */
+     624      312474 : AltGeneric::covariance_t AltGeneric::getCovarianceMatrix(void) const {
+     625      624415 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).P;
+     626             : }
+     627             : /*//}*/
+     628             : 
+     629             : /*//{ setCovarianceMatrix() */
+     630           0 : void AltGeneric::setCovarianceMatrix(const covariance_t &cov_in) {
+     631           0 :   mrs_lib::set_mutexed(mutex_sc_, cov_in, sc_.P);
+     632           0 : }
+     633             : /*//}*/
+     634             : 
+     635             : /*//{ getInnovation() */
+     636      210227 : double AltGeneric::getInnovation(const int &state_idx) const {
+     637      210227 :   return mrs_lib::get_mutexed(mtx_innovation_, innovation_)(state_idx);
+     638             : }
+     639             : 
+     640           0 : double AltGeneric::getInnovation(const int &state_id_in, const int &axis_in) const {
+     641           0 :   return getInnovation(stateIdToIndex(state_id_in, axis_in));
+     642             : }
+     643             : /*//}*/
+     644             : 
+     645             : /*//{ setDt() */
+     646      312801 : void AltGeneric::setDt(const double &dt) {
+     647      312801 :   dt_ = dt;
+     648      312801 :   generateA();
+     649      312706 :   generateB();
+     650      625408 :   std::scoped_lock lock(mutex_lkf_);
+     651      312700 :   lkf_->A = A_;
+     652      312640 :   lkf_->B = B_;
+     653      312575 : }
+     654             : /*//}*/
+     655             : 
+     656             : /*//{ setInputCoeff() */
+     657         382 : void AltGeneric::setInputCoeff(const double &input_coeff) {
+     658         382 :   input_coeff_ = input_coeff;
+     659         382 :   generateA();
+     660         376 :   generateB();
+     661         754 :   std::scoped_lock lock(mutex_lkf_);
+     662         380 :   lkf_->A = A_;
+     663         375 :   lkf_->B = B_;
+     664             : 
+     665         378 :   if (is_repredictor_enabled_) {
+     666           0 :     models_.clear();
+     667           0 :     generateRepredictorModels(input_coeff_);
+     668             :   }
+     669         379 : }
+     670             : /*//}*/
+     671             : 
+     672             : /*//{ generateRepredictorModels() */
+     673           0 : void AltGeneric::generateRepredictorModels(const double input_coeff) {
+     674             : 
+     675           0 :     for (int i = 0; i < alt_generic::n_states; i++) {
+     676             : 
+     677           0 :       auto lambda_generateA = [input_coeff](const double dt) {
+     678           0 :         A_t A;
+     679             :         // clang-format off
+     680           0 :         A <<
+     681           0 :           1, dt, 0.5 * dt * dt,
+     682           0 :           0, 1, dt,
+     683           0 :           0, 0, 1-(input_coeff * dt);
+     684             :         // clang-format on
+     685           0 :         return A;
+     686           0 :       };
+     687             : 
+     688           0 :       auto lambda_generateB = [input_coeff]([[maybe_unused]] const double dt) {
+     689           0 :         B_t B = B.Zero();
+     690             :         // clang-format off
+     691           0 :         B <<
+     692             :           0,
+     693           0 :           0,
+     694           0 :           (input_coeff * dt);
+     695             :         // clang-format on
+     696           0 :         return B;
+     697           0 :       };
+     698             : 
+     699           0 :       H_t H = H_t::Zero();
+     700           0 :       H(i)  = 1;
+     701           0 :       models_.push_back(std::make_shared<varstep_lkf_t>(lambda_generateA, lambda_generateB, H));
+     702             :     }
+     703           0 : }
+     704             : /*//}*/
+     705             : 
+     706             : /*//{ generateA() */
+     707      313381 : void AltGeneric::generateA() {
+     708             : 
+     709             :   // clang-format off
+     710      313381 :     A_ <<
+     711      313187 :       1, dt_, 0.5 * dt_ * dt_,
+     712      313248 :       0, 1, dt_,
+     713      313285 :       0, 0, 1-(input_coeff_ * dt_);
+     714             :   // clang-format on
+     715      313233 : }
+     716             : /*//}*/
+     717             : 
+     718             : /*//{ generateB() */
+     719      313376 : void AltGeneric::generateB() {
+     720             : 
+     721             :   // clang-format off
+     722      313376 :     B_ <<
+     723             :       0,
+     724      313104 :       0,
+     725      313162 :       (input_coeff_ * dt_);
+     726             :   // clang-format on
+     727      313127 : }
+     728             : /*//}*/
+     729             : 
+     730             : /*//{ callbackReconfigure() */
+     731         196 : void AltGeneric::callbackReconfigure(AltitudeEstimatorConfig &config, [[maybe_unused]] uint32_t level) {
+     732             : 
+     733         196 :   if (!isInitialized()) {
+     734         196 :     return;
+     735             :   }
+     736             : 
+     737           0 :   Q_t Q;
+     738           0 :   Q(POSITION, POSITION)         = config.pos;
+     739           0 :   Q(VELOCITY, VELOCITY)         = config.vel;
+     740           0 :   Q(ACCELERATION, ACCELERATION) = config.acc;
+     741           0 :   mrs_lib::set_mutexed(mtx_Q_, Q, Q_);
+     742             : }
+     743             : /*//}*/
+     744             : 
+     745             : /*//{ getNamespacedName() */
+     746        1262 : std::string AltGeneric::getNamespacedName() const {
+     747        2524 :   return parent_state_est_name_ + "/" + getName();
+     748             : }
+     749             : /*//}*/
+     750             : 
+     751             : /*//{ getPrintName() */
+     752        1529 : std::string AltGeneric::getPrintName() const {
+     753        3058 :   return ch_->nodelet_name + "/" + parent_state_est_name_ + "/" + getName();
+     754             : }
+     755             : /*//}*/
+     756             : };  // 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..83bab29753 --- /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..6de0c6ed562f0020e348f2b6ac1f1a3b6bf80a22 GIT binary patch literal 2923 zcmV-x3zYPUP)7#*!!f{)a0oWA3(ZC}!{Y0or(m-fx3MGDN9)Q@A~jiK&zJOLjHfTLb4={3{Cd5{S2m1&H~K|4fTJ5; z#=Oj_fcVsQjAQtDyi6y-F%lyye6+*>)UY*TfK}jT_M8cbPdx%VZ82_;^_iQguH)RN z#xTC2h7EsH!|2W9BiNyyjAq|#N(R#NvBDl~?0l>ZtCkIbknFVVj570XkPLsL$x^U` z>)}oadh4B?5trzHGKMS6Gv8<20@MnEi`o>ZWCa zMgzwaX_D`3$@a|q|MR+R|1ra}f8XC=fEC+wrIF$SNmh!^nH>bu8P%q6(?9Z4!Ap9) z$AVC_0+Ma2ia#Dga_&s)y`6-(n#kgtc_HvzP=N1$It@6%uL37<2?P~%QM%x;QlPo=CGLay06724DskQ@*N&>y9XPR<6nqXCF% zrF#4Rut|Ut!;^T>W{0FTMqsMs{!Wmq17bn$#e!ShYo>Nfta+TS76Sm8@Tmidp02Qu zl!7V)X1V&2{ba6MgFAx`=9=jq+~8d>6DWEV$r-jug;Wa7DS{bpq6#(BENoyDjEY-W!5Ge<3Sc(33DOxx23Saaf{gr{ zoxyDnkb*oMB!2R&?j+j|riFRl8KCBzSwQ4SYSlu3;pguRJJ@S+VRo>u;dEzi^d}E? zBBn@7ftV%swmsm&bI1~E3m69z@)E4{L}0TVLuY_12Dn5~O$rEcJZ6vG;y6%R7$7w8 zE$ogcAapeJE@hG_(iFR-#}YMSLLS4@Aq1ueAA!`_`zFQ8kcRPGlLC*FX!6%%9x?VT ztPl_yLl!R=0v`{`Q=Uicq5P7?KBp!ntDb9GC@9TO7{0@mGH@|_>N7WJVbc}@Qr47o zIo*a|(CQY49hzgo3S#9B^$P;=!tAJ0pnCL;nOc|KCvz#f7n-lJ%EeC;v-6t7AeM-zrFz^q#T?MZ;^*gg#?vzLKx#Z< zx`B4)6Kf20g<8#LuuMenrW70T0w8>`A+-tqx=m~^EMPoyi0Q&6ytZ8ks*q1c82!|V z1(4~Ido*QYzzAtbS^@P27PL(wTH=vj;@QH`@)%i)(%dn7iYFH{@|88h>?wD965tg& zL-#bFtSp!5&{4K>ZRozSM$)urw_ixy3fclXg^A<24kK~n| zI{#~#Js2E3T!&WX9;EJ)Gg4#PHkUQfwzRszolwq|s+m*LgrMAt;DX(AseA3b`=~KK z1U_k*R18%*Fs7J}v?7^tqqXofB!*CSPb{vrL7AQov&@(sv|JdTlFoveqP;kXUoWY< zriX`7+RmFV#@4wRb>kH?>_u^v0Bgn`?y|e8(?zq1Y^-UffJwp}2?aPBJ6!-#yQmL< z(S1^z#Gfh9{v?kZ^3ic;>K%wF)6|GjKWg?)uS#KE06ZFA3O(P`CD8|5l_Fv& zNRWKQPIM_2^38lS$B*Gi$aRcrHM5mHAHVlejfI82X643aoX{BgO}}?@0JUsF0Q?oI zjqd^r{dlJB8Ep4&(za>thka5KMQ@bje7MxN^7xK1my8b+;~s`sxj{4VHtOOU^E~bk zQ+BLAQ!LUF;Bd4A=MOe|V`|=y&nb#C?33uu95Ky2dsDis3{Ytc3-;09)2bXvAqT*w zS22qAo~?NCYsCF<vsRy#yq3CB$+Q1c6d#-w*i z$$}?Rgi{RZo8Gk7@c5vmxJn+|pN@B2J(Bp?@Np>43Y`sD>jXXk zkyYY=H*jYF&ZU6U0~V)^ z0UiI8Y*<1}4dadpX_N$LK4S~niEUGZZVL~HT0_+(TX;TTdHVw3H*Dcje*Pi>t^mu^ z?pbl{^Wm*UH*l|kgvQyAIL1FO4*WzkSNkyjBWx;wz{f=dK-gwA!B&HOQ7@Kcv6p!YVk2l{?K z2LObSPGRC0aL=6Ibn4%%~P0Z5dNNj!D}WvETx#pUyV(o9EJB{s*Oe VWmOf>UV8ul002ovPDHLkV1ldmd?f$? literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/src/estimators/altitude/index-detail-sort-f.html b/mrs_uav_state_estimators/src/estimators/altitude/index-detail-sort-f.html new file mode 100644 index 0000000000..5db155257e --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/index-detail-sort-f.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:21339653.8 %
Date:2024-12-17 22:35:29Functions: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.8%53.8%
+ 		53.8 %213 / 39666.7 %22 / 33
<unnamed>53.8 %213 / 39666.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..d987c68f03 --- /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:21339653.8 %
Date:2024-12-17 22:35:29Functions: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.8%53.8%
+ 		53.8 %213 / 39666.7 %22 / 33
<unnamed>53.8 %213 / 39666.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..c9bd09978a --- /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:21339653.8 %
Date:2024-12-17 22:35:29Functions: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.8%53.8%
+ 		53.8 %213 / 39666.7 %22 / 33
<unnamed>53.8 %213 / 39666.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..a78e8f744f --- /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:21339653.8 %
Date:2024-12-17 22:35:29Functions: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.8%53.8%
+ 		53.8 %213 / 39666.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..deefce00b7 --- /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:21339653.8 %
Date:2024-12-17 22:35:29Functions: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.8%53.8%
+ 		53.8 %213 / 39666.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..d848826e62 --- /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:21339653.8 %
Date:2024-12-17 22:35:29Functions: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.8%53.8%
+ 		53.8 %213 / 39666.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..0f334b4c13 --- /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:03580.0 %
Date:2024-12-17 22:35:29Functions: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..69abab8694 --- /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:03580.0 %
Date:2024-12-17 22:35:29Functions: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..264ec8eb4c --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.html @@ -0,0 +1,796 @@ + + + + + + + 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:03580.0 %
Date:2024-12-17 22:35:29Functions: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 :   changeState(INITIALIZED_STATE);
+     200           0 :   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
+     201             : 
+     202           0 :   return true;
+     203             : }
+     204             : /*//}*/
+     205             : 
+     206             : /* timerUpdate() //{*/
+     207           0 : void HdgGeneric::timerUpdate(const ros::TimerEvent &event) {
+     208             : 
+     209           0 :   if (!isInitialized()) {
+     210           0 :     return;
+     211             :   }
+     212             : 
+     213           0 :   switch (getCurrentSmState()) {
+     214             : 
+     215           0 :     case UNINITIALIZED_STATE: {
+     216           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s initialization", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     217           0 :       break;
+     218             :     }
+     219             : 
+     220           0 :     case READY_STATE: {
+     221           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s estimator start", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     222           0 :       break;
+     223             :     }
+     224             : 
+     225           0 :     case INITIALIZED_STATE: {
+     226             :       // initialize the estimator with current corrections
+     227           0 :       for (auto correction : corrections_) {
+     228           0 :         auto res = correction->getProcessedCorrection();
+     229           0 :         if (res) {
+     230           0 :           auto measurement_stamped = res.value();
+     231           0 :           setState(measurement_stamped.value(AXIS_X), correction->getStateId(), AXIS_X);
+     232           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: Setting initial state to: %.2f", getPrintName().c_str(), measurement_stamped.value(AXIS_X));
+     233             :         } else {
+     234           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(),
+     235             :                             correction->getNamespacedName().c_str());
+     236           0 :           return;
+     237             :         }
+     238             :       }
+     239           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     240           0 :       changeState(READY_STATE);
+     241           0 :       break;
+     242             :     }
+     243             : 
+     244           0 :     case STARTED_STATE: {
+     245           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     246           0 :       if (isConverged()) {
+     247           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: LKF converged", getPrintName().c_str());
+     248           0 :         changeState(RUNNING_STATE);
+     249             :       }
+     250           0 :       break;
+     251             :     }
+     252             : 
+     253           0 :     case RUNNING_STATE: {
+     254           0 :       for (auto correction : corrections_) {
+     255           0 :         if (!correction->isHealthy()) {
+     256           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     257           0 :           changeState(ERROR_STATE);
+     258             :         }
+     259             :       }
+     260           0 :       break;
+     261             :     }
+     262             : 
+     263           0 :     case STOPPED_STATE: {
+     264           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is stopped", getPrintName().c_str());
+     265           0 :       break;
+     266             :     }
+     267             : 
+     268           0 :     case ERROR_STATE: {
+     269           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is in ERROR state", getPrintName().c_str());
+     270           0 :       bool all_corrections_healthy = true;
+     271           0 :       for (auto correction : corrections_) {
+     272           0 :         if (!correction->isHealthy()) {
+     273           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     274           0 :           all_corrections_healthy = false;
+     275             :         }
+     276             :       }
+     277             :       // initialize the estimator again if corrections become healthy
+     278           0 :       if (all_corrections_healthy && innovation_ok_) {
+     279           0 :         changeState(INITIALIZED_STATE);
+     280             :       }
+     281           0 :       break;
+     282             :     }
+     283             :   }
+     284             : 
+     285           0 :   if (sh_control_input_.newMsg()) {
+     286           0 :     is_input_ready_ = true;
+     287             :   }
+     288             : 
+     289             :   // check age of input
+     290           0 :   if (is_input_ready_ && (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {
+     291           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: input too old (%.4f), using zero input instead", getPrintName().c_str(),
+     292             :                       (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec());
+     293           0 :     is_input_ready_ = false;
+     294             :   }
+     295             : 
+     296           0 :   if (!isRunning() && !isStarted()) {
+     297           0 :     return;
+     298             :   }
+     299             : 
+     300           0 :   if (first_iter_) {
+     301           0 :     first_iter_ = false;
+     302           0 :     return;
+     303             :   }
+     304             : 
+     305           0 :   double dt = (event.current_real - event.last_real).toSec();
+     306           0 :   if (dt <= 0.0 || dt > 1.0) {
+     307           0 :     return;
+     308             :   }
+     309             : 
+     310           0 :   if (!is_repredictor_enabled_) {  // repredictor calculates dt on its own
+     311           0 :     setDt(dt);
+     312             :   }
+     313             : 
+     314             :   // go through available corrections and apply them
+     315             :   /* for (auto correction : corrections_) { */
+     316             :   /*   auto res = correction->getProcessedCorrection(); */
+     317             :   /*   if (res) { */
+     318             :   /*     auto measurement_stamped = res.value(); */
+     319             :   /*     doCorrection(measurement_stamped.value, correction->getR(), correction->getStateId(), measurement_stamped.stamp); */
+     320             :   /*   } */
+     321             :   /* } */
+     322             : 
+     323             :   // prediction step
+     324           0 :   u_t       u;
+     325           0 :   ros::Time input_stamp;
+     326           0 :   if (is_input_ready_) {
+     327           0 :     input_stamp = sh_control_input_.getMsg()->header.stamp;
+     328           0 :     if (input_coeff_ != default_input_coeff_){
+     329           0 :       setInputCoeff(default_input_coeff_);
+     330             :     }
+     331           0 :     u(0) = sh_control_input_.getMsg()->control_hdg_rate;
+     332             :   } else {
+     333           0 :     input_stamp = ros::Time::now();
+     334           0 :     if (input_coeff_ != 0){
+     335           0 :       setInputCoeff(0);
+     336             :     }
+     337           0 :     u = u_t::Zero();
+     338             :   }
+     339             : 
+     340           0 :   statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+     341           0 :   Q_t        Q  = mrs_lib::get_mutexed(mtx_Q_, Q_);
+     342             :   try {
+     343             :     // Apply the prediction step
+     344           0 :     std::scoped_lock lock(mutex_lkf_);
+     345           0 :     if (is_repredictor_enabled_) {
+     346           0 :       lkf_rep_->addInputChangeWithNoise(u, Q, input_stamp, models_[0]);
+     347           0 :       sc = lkf_rep_->predictTo(ros::Time::now());
+     348             :     } else {
+     349           0 :       sc = lkf_->predict(sc, u, Q, dt_);
+     350             :     }
+     351             :   }
+     352           0 :   catch (const std::exception &e) {
+     353             :     // In case of error, alert the user
+     354           0 :     ROS_ERROR("[%s]: LKF prediction failed: %s", getPrintName().c_str(), e.what());
+     355             :   }
+     356             : 
+     357           0 :   mrs_lib::set_mutexed(mutex_sc_, sc, sc_);
+     358             : 
+     359           0 :   if (!isError()) {
+     360           0 :     mrs_lib::set_mutexed(mutex_last_valid_hdg_, sc.x(POSITION), last_valid_hdg_);
+     361             :   }
+     362             : 
+     363             :   // publishing
+     364           0 :   publishInput(u, input_stamp);
+     365           0 :   publishOutput();
+     366           0 :   publishDiagnostics();
+     367             : }
+     368             : /*//}*/
+     369             : 
+     370             : /*//{ timerCheckHealth() */
+     371           0 : void HdgGeneric::timerCheckHealth([[maybe_unused]] const ros::TimerEvent &event) {
+     372             : 
+     373           0 :   if (!isInitialized()) {
+     374           0 :     return;
+     375             :   }
+     376             : 
+     377           0 :   switch (getCurrentSmState()) {
+     378             : 
+     379           0 :     case UNINITIALIZED_STATE: {
+     380           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s initialization", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     381           0 :       break;
+     382             :     }
+     383             : 
+     384           0 :     case READY_STATE: {
+     385           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s estimator start", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     386           0 :       break;
+     387             :     }
+     388             : 
+     389           0 :     case INITIALIZED_STATE: {
+     390             :       // initialize the estimator with current corrections
+     391           0 :       for (auto correction : corrections_) {
+     392           0 :         auto res = correction->getProcessedCorrection();
+     393           0 :         if (res) {
+     394           0 :           auto measurement_stamped = res.value();
+     395           0 :           setState(measurement_stamped.value(AXIS_X), correction->getStateId(), AXIS_X);
+     396           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: Setting initial state to: %.2f", getPrintName().c_str(), measurement_stamped.value(AXIS_X));
+     397             :         } else {
+     398           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(),
+     399             :                             correction->getNamespacedName().c_str());
+     400           0 :           return;
+     401             :         }
+     402             :       }
+     403           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     404           0 :       changeState(READY_STATE);
+     405           0 :       break;
+     406             :     }
+     407             : 
+     408           0 :     case STARTED_STATE: {
+     409           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     410           0 :       if (isConverged()) {
+     411           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: LKF converged", getPrintName().c_str());
+     412           0 :         changeState(RUNNING_STATE);
+     413             :       }
+     414           0 :       break;
+     415             :     }
+     416             : 
+     417           0 :     case RUNNING_STATE: {
+     418           0 :       for (auto correction : corrections_) {
+     419           0 :         if (!correction->isHealthy()) {
+     420           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     421           0 :           changeState(ERROR_STATE);
+     422             :         }
+     423             :       }
+     424           0 :       break;
+     425             :     }
+     426             : 
+     427           0 :     case STOPPED_STATE: {
+     428           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is stopped", getPrintName().c_str());
+     429           0 :       break;
+     430             :     }
+     431             : 
+     432           0 :     case ERROR_STATE: {
+     433           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is in ERROR state", getPrintName().c_str());
+     434           0 :       bool all_corrections_healthy = true;
+     435           0 :       for (auto correction : corrections_) {
+     436           0 :         if (!correction->isHealthy()) {
+     437           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     438           0 :           all_corrections_healthy = false;
+     439             :         }
+     440             :       }
+     441             :       // initialize the estimator again if corrections become healthy
+     442           0 :       if (all_corrections_healthy) {
+     443           0 :         changeState(INITIALIZED_STATE);
+     444             :       }
+     445           0 :       break;
+     446             :     }
+     447             :   }
+     448             : 
+     449           0 :   if (sh_control_input_.newMsg()) {
+     450           0 :     is_input_ready_ = true;
+     451             :   }
+     452             : 
+     453             :   // check age of input
+     454           0 :   if (is_input_ready_ && (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {
+     455           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: input too old (%.4f), using zero input instead", getPrintName().c_str(),
+     456             :                       (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec());
+     457           0 :     is_input_ready_ = false;
+     458             :   }
+     459             : }
+     460             : /*//}*/
+     461             : 
+     462             : /*//{ doCorrection() */
+     463           0 : void HdgGeneric::doCorrection(const Correction<hdg_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t &state_id) {
+     464           0 :   doCorrection(meas.value, R, state_id, meas.stamp);
+     465           0 : }
+     466             : /*//}*/
+     467             : 
+     468             : /*//{ doCorrection() */
+     469           0 : void HdgGeneric::doCorrection(const z_t &z, const double R, const StateId_t &H_idx, const ros::Time &meas_stamp) {
+     470             : 
+     471           0 :   if (!isInitialized()) {
+     472           0 :     return;
+     473             :   }
+     474             : 
+     475             :   // copy measurement as we might need to modify it (unwrap)
+     476           0 :   z_t meas = z;
+     477             : 
+     478             :   // we do not want to perform corrections until the estimator is initialized
+     479           0 :   if (!(isInState(SMStates_t::READY_STATE) || isInState(SMStates_t::RUNNING_STATE) || isInState(SMStates_t::STARTED_STATE))) {
+     480           0 :     return;
+     481             :   }
+     482             : 
+     483           0 :   statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+     484             : 
+     485             :   // for position state check the innovation
+     486           0 :   if (H_idx == POSITION) {
+     487             : 
+     488             :     // unwrap the hdg measurement wrt current state
+     489           0 :     meas(POSITION) = mrs_lib::geometry::radians::unwrap(meas(POSITION), sc.x(POSITION));
+     490             : 
+     491           0 :     std::scoped_lock lock(mtx_innovation_);
+     492             : 
+     493           0 :     innovation_(0) = mrs_lib::geometry::radians::dist(mrs_lib::geometry::radians(meas(0)), mrs_lib::geometry::radians(sc.x(POSITION)));
+     494             : 
+     495           0 :     if (fabs(innovation_(0)) > pos_innovation_limit_) {
+     496           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: innovation too large - hdg: %.2f", getPrintName().c_str(), innovation_(0));
+     497           0 :       innovation_ok_ = false;
+     498           0 :       changeState(ERROR_STATE);
+     499             :     }
+     500             :   }
+     501             : 
+     502             :   try {
+     503             :     // Apply the correction step
+     504             :     {
+     505           0 :       std::scoped_lock lock(mutex_lkf_);
+     506           0 :       H_        = H_t::Zero();
+     507           0 :       H_(H_idx) = 1;
+     508           0 :       lkf_->H   = H_;
+     509           0 :       if (is_repredictor_enabled_) {
+     510           0 :         lkf_rep_->addMeasurement(meas, R_t::Ones() * R, meas_stamp, models_[H_idx]);
+     511             :       } else {
+     512           0 :         sc = lkf_->correct(sc, meas, R_t::Ones() * R);
+     513             :       }
+     514             :     }
+     515           0 :     innovation_ok_ = true;
+     516             :   }
+     517           0 :   catch (const std::exception &e) {
+     518             :     // In case of error, alert the user
+     519           0 :     ROS_ERROR("[%s]: LKF correction failed: %s", getPrintName().c_str(), e.what());
+     520             :   }
+     521             : 
+     522           0 :   mrs_lib::set_mutexed(mutex_sc_, sc, sc_);
+     523             : }
+     524             : /*//}*/
+     525             : 
+     526             : /*//{ isConverged() */
+     527           0 : bool HdgGeneric::isConverged() {
+     528             : 
+     529             :   // TODO: check convergence by rate of change of determinant
+     530             : 
+     531           0 :   return true;
+     532             : }
+     533             : /*//}*/
+     534             : 
+     535             : /*//{ getState() */
+     536           0 : double HdgGeneric::getState(const int &state_id_in, const int &axis_in) const {
+     537           0 :   return getState(stateIdToIndex(state_id_in, axis_in));
+     538             : }
+     539             : 
+     540           0 : double HdgGeneric::getState(const int &state_idx_in) const {
+     541           0 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).x(state_idx_in);
+     542             : }
+     543             : /*//}*/
+     544             : 
+     545             : /*//{ setState() */
+     546           0 : void HdgGeneric::setState(const double &state_in, const int &state_id_in, const int &axis_in) {
+     547           0 :   setState(state_in, stateIdToIndex(state_id_in, axis_in));
+     548           0 : }
+     549             : 
+     550           0 : void HdgGeneric::setState(const double &state_in, const int &state_idx_in) {
+     551           0 :   mrs_lib::set_mutexed(mutex_sc_, state_in, sc_.x(state_idx_in));
+     552           0 : }
+     553             : /*//}*/
+     554             : 
+     555             : /*//{ getStates() */
+     556           0 : HdgGeneric::states_t HdgGeneric::getStates(void) const {
+     557           0 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).x;
+     558             : }
+     559             : /*//}*/
+     560             : 
+     561             : /*//{ setStates() */
+     562           0 : void HdgGeneric::setStates(const states_t &states_in) {
+     563           0 :   mrs_lib::set_mutexed(mutex_sc_, states_in, sc_.x);
+     564           0 : }
+     565             : /*//}*/
+     566             : 
+     567             : /*//{ getCovariance() */
+     568           0 : double HdgGeneric::getCovariance(const int &state_id_in, const int &axis_in) const {
+     569           0 :   return getCovariance(stateIdToIndex(state_id_in, axis_in));
+     570             : }
+     571             : 
+     572           0 : double HdgGeneric::getCovariance(const int &state_idx_in) const {
+     573           0 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).P(state_idx_in, state_idx_in);
+     574             : }
+     575             : /*//}*/
+     576             : 
+     577             : /*//{ getCovarianceMatrix() */
+     578           0 : HdgGeneric::covariance_t HdgGeneric::getCovarianceMatrix(void) const {
+     579           0 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).P;
+     580             : }
+     581             : /*//}*/
+     582             : 
+     583             : /*//{ setCovarianceMatrix() */
+     584           0 : void HdgGeneric::setCovarianceMatrix(const covariance_t &cov_in) {
+     585           0 :   mrs_lib::set_mutexed(mutex_sc_, cov_in, sc_.P);
+     586           0 : }
+     587             : /*//}*/
+     588             : 
+     589             : /*//{ getInnovation() */
+     590           0 : double HdgGeneric::getInnovation(const int &state_idx) const {
+     591           0 :   return mrs_lib::get_mutexed(mtx_innovation_, innovation_)(state_idx);
+     592             : }
+     593             : 
+     594           0 : double HdgGeneric::getInnovation(const int &state_id_in, const int &axis_in) const {
+     595           0 :   return getInnovation(stateIdToIndex(state_id_in, axis_in));
+     596             : }
+     597             : /*//}*/
+     598             : 
+     599             : /*//{ setDt() */
+     600           0 : void HdgGeneric::setDt(const double &dt) {
+     601           0 :   dt_ = dt;
+     602           0 :   generateA();
+     603           0 :   generateB();
+     604           0 :   std::scoped_lock lock(mutex_lkf_);
+     605           0 :   lkf_->A = A_;
+     606           0 :   lkf_->B = B_;
+     607           0 : }
+     608             : /*//}*/
+     609             : 
+     610             : /*//{ setInputCoeff() */
+     611           0 : void HdgGeneric::setInputCoeff(const double &input_coeff) {
+     612           0 :   input_coeff_ = input_coeff;
+     613           0 :   generateA();
+     614           0 :   generateB();
+     615           0 :   std::scoped_lock lock(mutex_lkf_);
+     616           0 :   lkf_->A = A_;
+     617           0 :   lkf_->B = B_;
+     618             : 
+     619           0 :   if (is_repredictor_enabled_) {
+     620           0 :     models_.clear();
+     621           0 :     generateRepredictorModels(input_coeff_);
+     622             :   }
+     623           0 : }
+     624             : /*//}*/
+     625             : 
+     626             : /*//{ generateRepredictorModels() */
+     627           0 : void HdgGeneric::generateRepredictorModels(const double input_coeff) {
+     628             : 
+     629           0 :     for (int i = 0; i < hdg_generic::n_states; i++) {
+     630             : 
+     631           0 :       auto lambda_generateA = [input_coeff](const double dt) {
+     632           0 :         A_t A;
+     633             :         // clang-format off
+     634           0 :         A <<
+     635           0 :           1, dt,
+     636           0 :           0, 1-(input_coeff * dt);
+     637             :         // clang-format on
+     638           0 :         return A;
+     639           0 :       };
+     640             : 
+     641           0 :       auto lambda_generateB = [input_coeff]([[maybe_unused]] const double dt) {
+     642           0 :         B_t B = B.Zero();
+     643             :         // clang-format off
+     644           0 :         B <<
+     645             :           0,
+     646           0 :           (input_coeff * dt);
+     647             :         // clang-format on
+     648           0 :         return B;
+     649           0 :       };
+     650             : 
+     651           0 :       H_t H = H_t::Zero();
+     652           0 :       H(i)  = 1;
+     653           0 :       models_.push_back(std::make_shared<varstep_lkf_t>(lambda_generateA, lambda_generateB, H));
+     654             :     }
+     655           0 : }
+     656             : /*//}*/
+     657             : 
+     658             : /*//{ generateA() */
+     659           0 : void HdgGeneric::generateA() {
+     660             : 
+     661             :   // clang-format off
+     662           0 :     A_ <<
+     663           0 :       1, dt_,
+     664           0 :       0, 1-(input_coeff_ * dt_);
+     665             :   // clang-format on
+     666           0 : }
+     667             : /*//}*/
+     668             : 
+     669             : /*//{ generateB() */
+     670           0 : void HdgGeneric::generateB() {
+     671             : 
+     672             :   // clang-format off
+     673           0 :     B_ <<
+     674             :       0,
+     675           0 :       (input_coeff_ * dt_);
+     676             :   // clang-format on
+     677           0 : }
+     678             : /*//}*/
+     679             : 
+     680             : /*//{ getLastValidHdg() */
+     681           0 : double HdgGeneric::getLastValidHdg() const {
+     682           0 :   return mrs_lib::get_mutexed(mutex_last_valid_hdg_, last_valid_hdg_);
+     683             : }
+     684             : /*//}*/
+     685             : 
+     686             : /*//{ callbackReconfigure() */
+     687           0 : void HdgGeneric::callbackReconfigure(HeadingEstimatorConfig &config, [[maybe_unused]] uint32_t level) {
+     688             : 
+     689           0 :   if (!isInitialized()) {
+     690           0 :     return;
+     691             :   }
+     692             : 
+     693           0 :   Q_t Q;
+     694           0 :   Q(POSITION, POSITION) = config.pos;
+     695           0 :   Q(VELOCITY, VELOCITY) = config.vel;
+     696           0 :   mrs_lib::set_mutexed(mtx_Q_, Q, Q_);
+     697             : }
+     698             : /*//}*/
+     699             : 
+     700             : /*//{ getNamespacedName() */
+     701           0 : std::string HdgGeneric::getNamespacedName() const {
+     702           0 :   return parent_state_est_name_ + "/" + getName();
+     703             : }
+     704             : /*//}*/
+     705             : 
+     706             : /*//{ getPrintName() */
+     707           0 : std::string HdgGeneric::getPrintName() const {
+     708           0 :   return ch_->nodelet_name + "/" + parent_state_est_name_ + "/" + getName();
+     709             : }
+     710             : /*//}*/
+     711             : 
+     712             : };  // 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..b2c7da63ee --- /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..f3bcd18d64053a8bee215252b27280e4b4803f09 GIT binary patch literal 2312 zcmV+j3HSDiP)RGaN<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+& zkDdn;9#e7-S7or3GN6oL2%pH!PKb}C1T`k?3g(vlO+a^6e1sa^r=W)x z8V53hQ}4C+!%i_}iYYr8j5SkjZ&xCohGD8o_%Gj+v&)h+ou|8hL*kH<;b=IRL z%=>x1Rl#%glXJnbC$#z@GPSR|;u(LHqo1`wS%4XLu83d36tUZEpmQ$IF?H^$zH2pD zS!<4Zf@CoK?6|-pE@vFq;oG%KVR!_%!&1;fvk;`L+nUt1g!>UPNQ1Vp>@s2BV>b;H z#6l%#pRX}E9#Vi)X}1c~I^JEGOrO^^0^QRV7r^meOJS?_Il2@sS7v^{PKc;-<3L!Q zocWCRPQdj?Ly6IjL z1Hzx{)l0)gnc`(G_2cTeE^db|B}oP0VN@WOIZA8bJo99&ZK`h#stT7K_vFCT%cxrM zYGrm?7Z}utsO~@75HUUVu=IJoXOb?KCWXGS;V*e7&s-tn;@5t;Dx8a(U1=p0|Kg3h z)Sl!DFz;o*;JW4}W*H6n58)~?X$Y5BX^LjnuP$VH!hXD(Pk!qDfAi+S=p(_iNJ^n_0%w{p+IjV81R}5BF=bwJB4> zT95k)h47@JwCzCA<}4Aow!JmeH3EF60+cbPDcZ`Zx>5r>Ep56k0fYx`0y`;Bg=Cn) zBU8M>0NHR1eQ<&B=p5IOlZnsYMoYm(gQPpY5@GgCDwj1SvzQj1-%SzPXY*YE4Zgx< z&hk7*`o>wAJ<@tHwJ?L)UXoTfkBI)7@X8(%$tQ+8eAh!FXr=9>r-!7-`1FvT9umXx z=^;Hmq)x`|`laynkT}Aphcq_$(?fcCNIl1!0QVbuNK%k|dPqa~K0TxgJ7!Sf+gG% z!>uDkw_(^b1mHs%(a`+=Y&PAY_o3aq*dTn*?uGdpiO&p3lJb$BU-7_FdO?H6cWW%3 z-_Rd3dVsivAg$5*Sd zr-Wu1{)6~PWpu|Zzh%c-cglQ+*1WFCe<=X5tI%!zLi+YqSOr~a z71oX6DXrl#kn!wDaTXruUkDNoJ)DdaFKqYYctYi19|;jDal-FdTugLio<3<_d_O8R z{UsNw!WEA2*>$W0Lr4clh*lMDfky?ELUbgkOdC6@jENlx>#8EVZ3{m$97!{tK0V~w z+KO;lCgB|;PcuV63@p1AXe#AT2@ZUc(@q1IGS~*9U zAwGDcBm!^qxbRRHLQ|Wx3e}51U*tm7i(8kbwB^67^&p`k!SMRy-lydU8fxRu|JF?x zD1$TknBvl65CxtkERd11v%8>YxM+&fHELSNw`-=nsPdLttGB~Ct=UjfYi(_FJ*1f6 iQmx}gLmztQruYw5b+`w)y)w7}0000 + + + + + + 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-12-17 22:35:29Functions: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&)116
mrs_uav_state_estimators::HdgPassthrough::start()186
mrs_uav_state_estimators::HdgPassthrough::getNamespacedName[abi:cxx11]() const232
mrs_uav_state_estimators::HdgPassthrough::getPrintName[abi:cxx11]() const663
mrs_uav_state_estimators::HdgPassthrough::getCovarianceMatrix() const218285
mrs_uav_state_estimators::HdgPassthrough::getStates() const218295
mrs_uav_state_estimators::HdgPassthrough::timerUpdate(ros::TimerEvent const&)218299
mrs_uav_state_estimators::HdgPassthrough::timerCheckHealth(ros::TimerEvent const&)222556
mrs_uav_state_estimators::HdgPassthrough::callbackAngularVelocity(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)228680
mrs_uav_state_estimators::HdgPassthrough::callbackOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)231432
mrs_uav_state_estimators::HdgPassthrough::setState(double const&, int const&)460038
mrs_uav_state_estimators::HdgPassthrough::getState(int const&) const764632
+
+
+ + + +
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..44858f27c5 --- /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-12-17 22:35:29Functions: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&)116
mrs_uav_state_estimators::HdgPassthrough::timerUpdate(ros::TimerEvent const&)218299
mrs_uav_state_estimators::HdgPassthrough::timerCheckHealth(ros::TimerEvent const&)222556
mrs_uav_state_estimators::HdgPassthrough::callbackOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)231432
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>)228680
mrs_uav_state_estimators::HdgPassthrough::pause()0
mrs_uav_state_estimators::HdgPassthrough::reset()0
mrs_uav_state_estimators::HdgPassthrough::start()186
mrs_uav_state_estimators::HdgPassthrough::setState(double const&, int const&)460038
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]() const663
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]() const232
mrs_uav_state_estimators::HdgPassthrough::getCovarianceMatrix() const218285
mrs_uav_state_estimators::HdgPassthrough::getState(int const&) const764632
mrs_uav_state_estimators::HdgPassthrough::getState(int const&, int const&) const0
mrs_uav_state_estimators::HdgPassthrough::getStates() const218295
+
+
+ + + +
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..86c2d442f5 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.html @@ -0,0 +1,393 @@ + + + + + + + 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-12-17 22:35:29Functions: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         116 : void HdgPassthrough::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      15             : 
+      16         116 :   ch_ = ch;
+      17         116 :   ph_ = ph;
+      18             : 
+      19         116 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      20             : 
+      21         116 :   hdg_state_      = states_t::Zero();
+      22         116 :   hdg_covariance_ = covariance_t::Zero();
+      23             : 
+      24         116 :   innovation_ << 
+      25         116 :     0, 0;
+      26             : 
+      27             : 
+      28             :   // | --------------- param loader initialization --------------- |
+      29             : 
+      30         116 :   if (is_core_plugin_) {
+      31             : 
+      32         116 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      33         116 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      34             :   }
+      35             : 
+      36         116 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      37             : 
+      38             :   // | --------------------- load parameters -------------------- |
+      39         116 :   ph->param_loader->loadParam("max_flight_z", max_flight_z_);
+      40             : 
+      41         116 :   ph->param_loader->loadParam("topics/orientation", orient_topic_);
+      42         116 :   ph->param_loader->loadParam("topics/angular_velocity", ang_vel_topic_);
+      43             : 
+      44         116 :   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         116 :   timer_update_       = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &HdgPassthrough::timerUpdate, this, false, false);  // not running after init
+      51         116 :   timer_check_health_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &HdgPassthrough::timerCheckHealth, this);
+      52             : 
+      53             :   // | --------------- subscribers initialization --------------- |
+      54             :   // subscriber to odometry
+      55         232 :   mrs_lib::SubscribeHandlerOptions shopts;
+      56         116 :   shopts.nh                 = nh;
+      57         116 :   shopts.node_name          = getPrintName();
+      58         116 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      59         116 :   shopts.threadsafe         = true;
+      60         116 :   shopts.autostart          = true;
+      61         116 :   shopts.queue_size         = 10;
+      62         116 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      63             : 
+      64         232 :   sh_orientation_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, "/" + ch_->uav_name + "/" + orient_topic_,
+      65         116 :                                                                                 &HdgPassthrough::callbackOrientation, this);
+      66         232 :   sh_ang_vel_     = mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped>(shopts, "/" + ch_->uav_name + "/" + ang_vel_topic_,
+      67         116 :                                                                          &HdgPassthrough::callbackAngularVelocity, this);
+      68             : 
+      69             :   // | ---------------- publishers initialization --------------- |
+      70         116 :   if (ch_->debug_topics.output) {
+      71         116 :     ph_output_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput>(nh, getNamespacedName() + "/output", 10);
+      72             :   }
+      73         116 :   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         116 :   if (changeState(INITIALIZED_STATE)) {
+      80         116 :     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         116 : }
+      85             : /*//}*/
+      86             : 
+      87             : /*//{ start() */
+      88         186 : bool HdgPassthrough::start(void) {
+      89             : 
+      90         186 :   if (isInState(READY_STATE)) {
+      91         116 :     timer_update_.start();
+      92         116 :     changeState(STARTED_STATE);
+      93         116 :     return true;
+      94             : 
+      95             :   } else {
+      96          70 :     ROS_WARN_THROTTLE(1.0, "[%s]: Estimator must be in READY_STATE to start it", getPrintName().c_str());
+      97          70 :     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             :   // nothing to do during reset of passthrough estimator 
+     125           0 :   changeState(INITIALIZED_STATE);
+     126             : 
+     127           0 :   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
+     128             : 
+     129             :   return true;
+     130             : }
+     131             : /*//}*/
+     132      231432 : 
+     133             : /*//{ callbackOrientation() */
+     134      231432 : void HdgPassthrough::callbackOrientation(const geometry_msgs::QuaternionStamped::ConstPtr msg) {
+     135          28 : 
+     136             :   if (!isInitialized()) {
+     137             :     return;
+     138             :   }
+     139             : 
+     140      231393 :   double hdg;
+     141             :   try {
+     142           0 :     hdg = mrs_lib::AttitudeConverter(msg->quaternion).getHeading();
+     143           0 :   }
+     144             :   catch (...) {
+     145             :     ROS_ERROR_THROTTLE(1.0, "[%s]: failed getting heading", getPrintName().c_str());
+     146      231392 :   }
+     147             : 
+     148      231363 :   setState(hdg, POSITION);
+     149      231330 : 
+     150             :   if (!isError()) {
+     151             :     mrs_lib::set_mutexed(mutex_last_valid_hdg_, hdg, last_valid_hdg_);
+     152             :   }
+     153             : }
+     154             : /*//}*/
+     155      228680 : 
+     156             : /*//{ callbackAngularVelocity() */
+     157      228680 : void HdgPassthrough::callbackAngularVelocity(const geometry_msgs::Vector3Stamped::ConstPtr msg) {
+     158           1 : 
+     159             :   if (!isInitialized() || !sh_orientation_.hasMsg()) {
+     160             :     return;
+     161             :   }
+     162             : 
+     163      228657 :   double hdg_rate;
+     164             :   try {
+     165           0 :     hdg_rate = mrs_lib::AttitudeConverter(sh_orientation_.getMsg()->quaternion).getHeadingRate(msg->vector);
+     166           0 :   }
+     167             :   catch (...) {
+     168             :     ROS_ERROR_THROTTLE(1.0, "[%s]: failed getting heading", getPrintName().c_str());
+     169      228410 :   }
+     170             : 
+     171             :   setState(hdg_rate, VELOCITY);
+     172             : }
+     173             : /*//}*/
+     174      218299 : 
+     175             : /* timerUpdate() //{*/
+     176             : void HdgPassthrough::timerUpdate([[maybe_unused]] const ros::TimerEvent &event) {
+     177      218299 : 
+     178           0 : 
+     179             :   if (!isInitialized()) {
+     180             :     return;
+     181      218266 :   }
+     182      218305 : 
+     183             :   publishOutput();
+     184             :   publishDiagnostics();
+     185             : }
+     186             : /*//}*/
+     187      222556 : 
+     188             : /*//{ timerCheckHealth() */
+     189      222556 : void HdgPassthrough::timerCheckHealth([[maybe_unused]] const ros::TimerEvent &event) {
+     190           1 : 
+     191             :   if (!isInitialized()) {
+     192             :     return;
+     193      222547 :   }
+     194             : 
+     195         116 :   if (isInState(INITIALIZED_STATE) && is_orient_ready_ && is_ang_vel_ready_) {
+     196         116 : 
+     197             :     changeState(READY_STATE);
+     198             :     ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     199      222545 :   }
+     200             : 
+     201         116 :   if (isInState(STARTED_STATE)) {
+     202         116 : 
+     203             :     ROS_INFO_THROTTLE(1.0, "[%s]: Estimator Running", getPrintName().c_str());
+     204             :     changeState(RUNNING_STATE);
+     205      222552 :   }
+     206      220554 : 
+     207             :   if (sh_orientation_.hasMsg()) {
+     208        2001 :     is_orient_ready_ = true;
+     209             :   } else {
+     210             :     ROS_WARN_THROTTLE(1.0, "[%s]: has not received orientation yet", getPrintName().c_str());
+     211      222560 :   }
+     212      218663 : 
+     213             :   if (sh_ang_vel_.hasMsg()) {
+     214        3870 :     is_ang_vel_ready_ = true;
+     215             :   } else {
+     216             :     ROS_WARN_THROTTLE(1.0, "[%s]: has not received angular velocity yet", getPrintName().c_str());
+     217             :   }
+     218             : }
+     219             : /*//}*/
+     220           0 : 
+     221           0 : /*//{ getState() */
+     222             : double HdgPassthrough::getState(const int &state_id_in, const int &axis_in) const {
+     223             :   return getState(stateIdToIndex(state_id_in, axis_in));
+     224      764632 : }
+     225      764632 : 
+     226             : double HdgPassthrough::getState(const int &state_idx_in) const {
+     227             :   return mrs_lib::get_mutexed(mtx_hdg_state_, hdg_state_(state_idx_in));
+     228             : }
+     229             : /*//}*/
+     230           0 : 
+     231           0 : /*//{ setState() */
+     232           0 : void HdgPassthrough::setState(const double &state_in, const int &state_id_in, const int &axis_in) {
+     233             :   setState(state_in, stateIdToIndex(state_id_in, axis_in));
+     234      460038 : }
+     235             : 
+     236      460038 : void HdgPassthrough::setState(const double &state_in, const int &state_idx_in) {
+     237      459834 : 
+     238      459849 :   const double prev_hdg_state   = mrs_lib::get_mutexed(mtx_hdg_state_, hdg_state_(state_idx_in));
+     239             :   prev_hdg_state_(state_idx_in) = prev_hdg_state;
+     240      459819 :   mrs_lib::set_mutexed(mtx_hdg_state_, state_in, hdg_state_(state_idx_in));
+     241      459614 : 
+     242      459830 :   const double innovation = mrs_lib::geometry::radians::dist(mrs_lib::geometry::radians(state_in), mrs_lib::geometry::radians(prev_hdg_state));
+     243             :   mrs_lib::set_mutexed(mtx_innovation_, innovation, innovation_(state_idx_in));
+     244             : }
+     245             : /*//}*/
+     246      218295 : 
+     247      218295 : /*//{ getStates() */
+     248             : HdgPassthrough::states_t HdgPassthrough::getStates(void) const {
+     249             :   return mrs_lib::get_mutexed(mtx_hdg_state_, hdg_state_);
+     250             : }
+     251             : /*//}*/
+     252           0 : 
+     253           0 : /*//{ setStates() */
+     254           0 : void HdgPassthrough::setStates(const states_t &states_in) {
+     255             :   mrs_lib::set_mutexed(mtx_hdg_state_, states_in, hdg_state_);
+     256             : }
+     257             : /*//}*/
+     258           0 : 
+     259           0 : /*//{ getCovariance() */
+     260             : double HdgPassthrough::getCovariance(const int &state_id_in, const int &axis_in) const {
+     261             :   return getCovariance(stateIdToIndex(state_id_in, axis_in));
+     262           0 : }
+     263           0 : 
+     264             : double HdgPassthrough::getCovariance(const int &state_idx_in) const {
+     265             :   return mrs_lib::get_mutexed(mtx_hdg_covariance_, hdg_covariance_(state_idx_in, state_idx_in));
+     266             : }
+     267             : /*//}*/
+     268      218285 : 
+     269      218285 : /*//{ getCovarianceMatrix() */
+     270             : HdgPassthrough::covariance_t HdgPassthrough::getCovarianceMatrix(void) const {
+     271             :   return mrs_lib::get_mutexed(mtx_hdg_covariance_, hdg_covariance_);
+     272             : }
+     273             : /*//}*/
+     274           0 : 
+     275           0 : /*//{ setCovarianceMatrix() */
+     276           0 : void HdgPassthrough::setCovarianceMatrix(const covariance_t &cov_in) {
+     277             :   mrs_lib::set_mutexed(mtx_hdg_covariance_, cov_in, hdg_covariance_);
+     278             : }
+     279             : /*//}*/
+     280           0 : 
+     281           0 : /*//{ getInnovation() */
+     282             : double HdgPassthrough::getInnovation(const int &state_idx) const {
+     283             :   return mrs_lib::get_mutexed(mtx_innovation_, innovation_(state_idx));
+     284           0 : }
+     285           0 : 
+     286             : double HdgPassthrough::getInnovation(const int &state_id_in, const int &axis_in) const {
+     287             :   return getInnovation(stateIdToIndex(state_id_in, axis_in));
+     288             : }
+     289             : /*//}*/
+     290           0 : 
+     291           0 : /*//{ getLastValidHdg() */
+     292             : double HdgPassthrough::getLastValidHdg() const {
+     293             :   return mrs_lib::get_mutexed(mutex_last_valid_hdg_, last_valid_hdg_);
+     294             : }
+     295             : /*//}*/
+     296         232 : 
+     297         464 : /*//{ getNamespacedName() */
+     298             : std::string HdgPassthrough::getNamespacedName() const {
+     299             :   return parent_state_est_name_ + "/" + getName();
+     300             : }
+     301             : /*//}*/
+     302         663 : 
+     303        1326 : /*//{ getPrintName() */
+     304             : std::string HdgPassthrough::getPrintName() const {
+     305             :   return ch_->nodelet_name + "/" + parent_state_est_name_ + "/" + getName();
+     306             : }
+     307             : /*//}*/
+     308             : 
+     309             : };  // 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..621732d2c9 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.overview.html @@ -0,0 +1,98 @@ + + + + + + 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 + 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..cce437d1efda131ed830a5e3a0984056140fb5c8 GIT binary patch literal 1239 zcmV;|1StE7P)Uy%C4u4QKmxL-UnZag-K#DS{r>UBY$X1 zO2xnKZ9d4T#N(ZgmLZ2eRWcOo=+N5q85g4%W*iuLB+dd_C;O7=llwawxJwE;b&oR3 zVtQnwWPp)l?IAlhMn3VO;f(6-e%3c%pH!^kx_P5|X3 z;~afOlCEVKfx_w0W|+nJY*&&b(K9A%wHR{?H5s*+*ciJUy344!%2=VL7jS}c*0(Ze# zijau2eMh1qg#|_)9g8kzcZ%kyQdlbO&7rjY9Ou|`I6qUcLDs3M7l<8Z7(&OV7G`+R zIvKIVtp!TZn>og|#E(OIe`xL;+0he*?qwE!?)mS7q8SigBIxY5J+$>3xcX#&yJy(M zjN72~xSWItcb3c%gX5aHqaNgnMB(~~zIpDCaCTKNu*V?wURwKR#m!6AwOv{l z+bVGNzI|WORk(invU__htq&B^-R%~_jElmLgG!g(*)eN!(*}9^YM`@cQe3e!8YH$v z)eoH|L{CW)Yz_@Z&s*S10Ja6!;`SxDD!k6cjrM@v55biVF|Q0Cu4nyODa5+!`Y*vX zjqEnDe(u2spb!o{I^Gto12zeEN$m94L-Ra6(!{Y1^jK={V+MpHJ=2%uN9v5IogdLT zp!#Y%^MtQ6ty3lkIbB^oY-&$fplmjI^i4Acr~D1AsjW};&^TVw&K;_ZMf1`2`d77% znvl7azyDbJTDnvlk4&bgDi_k4z#-6Dw~r~7+q9li!TDyR$*x>s`MY-d6tU-9AL(Rq zi|>z1SPhp&c+Qr{?7e0tpWT$s@}7=BRlvLC90UDDo+%vTZ*9m)U@s-x!~)AvEAa`> z6prz?G;-2VI}>G`xUL+@+3)gm*LWIOK0DKpfYa+>%a!yb&J1fqYjBPC-q3RO9^4Uy z0r6)h3gH}IfWoc!m_&iX_^1V56BIj|0IVk^#usSa)4Ng?cK6sx(96GX_A9ELB+JyaJUDy@sZgI8*~)>57?ka8lu zLTmbA^}ei002ovPDHLkV1l2F BSh@fJ literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/src/estimators/heading/index-detail-sort-f.html b/mrs_uav_state_estimators/src/estimators/heading/index-detail-sort-f.html new file mode 100644 index 0000000000..4354bd1ac9 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/index-detail-sort-f.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:9149118.5 %
Date:2024-12-17 22:35:29Functions: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 / 3580.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..1df680ee0c --- /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:9149118.5 %
Date:2024-12-17 22:35:29Functions: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 / 3580.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..376f557a3f --- /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:9149118.5 %
Date:2024-12-17 22:35:29Functions: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 / 3580.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..d9c0d56d0a --- /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:9149118.5 %
Date:2024-12-17 22:35:29Functions: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 / 3580.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..f8886aa833 --- /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:9149118.5 %
Date:2024-12-17 22:35:29Functions: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 / 3580.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..56be5c3329 --- /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:9149118.5 %
Date:2024-12-17 22:35:29Functions: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 / 3580.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..24bdcdbe63 --- /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:24143855.0 %
Date:2024-12-17 22:35:29Functions: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.0%55.0%
+ 		55.0 %241 / 43875.8 %25 / 33
<unnamed>55.0 %241 / 43875.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..c698bf2678 --- /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:24143855.0 %
Date:2024-12-17 22:35:29Functions: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.0%55.0%
+ 		55.0 %241 / 43875.8 %25 / 33
<unnamed>55.0 %241 / 43875.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..0b95159ca9 --- /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:24143855.0 %
Date:2024-12-17 22:35:29Functions: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.0%55.0%
+ 		55.0 %241 / 43875.8 %25 / 33
<unnamed>55.0 %241 / 43875.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..a0d682882c --- /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:24143855.0 %
Date:2024-12-17 22:35:29Functions: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.0%55.0%
+ 		55.0 %241 / 43875.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..c7a4909809 --- /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:24143855.0 %
Date:2024-12-17 22:35:29Functions: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.0%55.0%
+ 		55.0 %241 / 43875.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..d33389d90a --- /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:24143855.0 %
Date:2024-12-17 22:35:29Functions: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.0%55.0%
+ 		55.0 %241 / 43875.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..ccd74d0493 --- /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:24143855.0 %
Date:2024-12-17 22:35:29Functions: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&)116
mrs_uav_state_estimators::LatGeneric::isConverged()116
mrs_uav_state_estimators::LatGeneric::callbackReconfigure(mrs_uav_state_estimators::LateralEstimatorConfig&, unsigned int)116
mrs_uav_state_estimators::LatGeneric::setInputCoeff(double const&)225
mrs_uav_state_estimators::LatGeneric::setState(double const&, int const&)376
mrs_uav_state_estimators::LatGeneric::setState(double const&, int const&, int const&)376
mrs_uav_state_estimators::LatGeneric::getNamespacedName[abi:cxx11]() const700
mrs_uav_state_estimators::LatGeneric::getPrintName[abi:cxx11]() const1084
mrs_uav_state_estimators::LatGeneric::start()4330
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) const7750
mrs_uav_state_estimators::LatGeneric::getCovarianceMatrix() const198457
mrs_uav_state_estimators::LatGeneric::setDt(double const&)198459
mrs_uav_state_estimators::LatGeneric::getStates() const198463
mrs_uav_state_estimators::LatGeneric::generateA()198801
mrs_uav_state_estimators::LatGeneric::generateB()198804
mrs_uav_state_estimators::LatGeneric::timerUpdate(ros::TimerEvent const&)222208
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&)229246
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) const229380
mrs_uav_state_estimators::LatGeneric::doCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)229399
mrs_uav_state_estimators::LatGeneric::getInnovation(int const&) const420407
mrs_uav_state_estimators::LatGeneric::getInnovation(int const&, int const&) const420424
mrs_uav_state_estimators::LatGeneric::getCovariance(int const&) const840840
mrs_uav_state_estimators::LatGeneric::getCovariance(int const&, int const&) const840863
mrs_uav_state_estimators::LatGeneric::getState(int const&) const1705029
mrs_uav_state_estimators::LatGeneric::getState(int const&, int const&) const1705391
+
+
+ + + +
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..3d527703fd --- /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:24143855.0 %
Date:2024-12-17 22:35:29Functions: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&)116
mrs_uav_state_estimators::LatGeneric::isConverged()116
mrs_uav_state_estimators::LatGeneric::timerUpdate(ros::TimerEvent const&)222208
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&)229246
mrs_uav_state_estimators::LatGeneric::doCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)229399
mrs_uav_state_estimators::LatGeneric::setInputCoeff(double const&)225
mrs_uav_state_estimators::LatGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::LatGeneric::callbackReconfigure(mrs_uav_state_estimators::LateralEstimatorConfig&, unsigned int)116
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&)198459
mrs_uav_state_estimators::LatGeneric::start()4330
mrs_uav_state_estimators::LatGeneric::setState(double const&, int const&)376
mrs_uav_state_estimators::LatGeneric::setState(double const&, int const&, int const&)376
mrs_uav_state_estimators::LatGeneric::generateA()198801
mrs_uav_state_estimators::LatGeneric::generateB()198804
mrs_uav_state_estimators::LatGeneric::setStates(Eigen::Matrix<double, 6, 1, 0, 6, 1> const&)0
mrs_uav_state_estimators::LatGeneric::getPrintName[abi:cxx11]() const1084
mrs_uav_state_estimators::LatGeneric::getCovariance(int const&) const840840
mrs_uav_state_estimators::LatGeneric::getCovariance(int const&, int const&) const840863
mrs_uav_state_estimators::LatGeneric::getInnovation(int const&) const420407
mrs_uav_state_estimators::LatGeneric::getInnovation(int const&, int const&) const420424
mrs_uav_state_estimators::LatGeneric::getNamespacedName[abi:cxx11]() const700
mrs_uav_state_estimators::LatGeneric::getCovarianceMatrix() const198457
mrs_uav_state_estimators::LatGeneric::getState(int const&) const1705029
mrs_uav_state_estimators::LatGeneric::getState(int const&, int const&) const1705391
mrs_uav_state_estimators::LatGeneric::getStates() const198463
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) const229380
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) const7750
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..64fac2b147 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.html @@ -0,0 +1,888 @@ + + + + + + + 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:24143855.0 %
Date:2024-12-17 22:35:29Functions: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         116 : void LatGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      14             : 
+      15         116 :   ch_ = ch;
+      16         116 :   ph_ = ph;
+      17             : 
+      18         116 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      19             : 
+      20             :   // clang-format off
+      21         116 :   dt_ = 0.01;
+      22         116 :   input_coeff_ = 10;
+      23         116 :   default_input_coeff_ = 10;
+      24             : 
+      25         116 :   generateA();
+      26         116 :   generateB();
+      27             : 
+      28         116 :   H_ <<
+      29         116 :     1, 0, 0, 0, 0, 0,
+      30         116 :     0, 1, 0, 0, 0, 0;
+      31             : 
+      32         116 :   innovation_ << 
+      33         116 :     0, 0;
+      34             : 
+      35             : 
+      36             :   // clang-format on
+      37             : 
+      38             :   // | --------------- initialize parameter loader -------------- |
+      39             : 
+      40         116 :   if (is_core_plugin_) {
+      41             : 
+      42         116 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      43         116 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      44             :   }
+      45             : 
+      46         116 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      47             : 
+      48             :   // | --------------------- load parameters -------------------- |
+      49         116 :   ph->param_loader->loadParam("hdg_source_topic", hdg_source_topic_);
+      50         116 :   ph->param_loader->loadParam("max_flight_z", max_flight_z_);
+      51         116 :   ph->param_loader->loadParam("innovation/limit", pos_innovation_limit_);
+      52         116 :   ph->param_loader->loadParam("innovation/action", exc_innovation_action_name_);
+      53         116 :   exc_innovation_action_ = map_exc_inno_action.at(exc_innovation_action_name_);
+      54         116 :   ph->param_loader->loadParam("repredictor/enabled", is_repredictor_enabled_);
+      55         116 :   if (is_repredictor_enabled_) {
+      56           0 :     ph->param_loader->loadParam("repredictor/buffer_size", rep_buffer_size_);
+      57             :   }
+      58             : 
+      59             :   // | --------------- corrections initialization --------------- |
+      60         116 :   ph->param_loader->loadParam("corrections", correction_names_);
+      61             : 
+      62         236 :   for (auto corr_name : correction_names_) {
+      63         120 :     corrections_.push_back(std::make_shared<Correction<lat_generic::n_measurements>>(
+      64        7990 :         nh, getNamespacedName(), corr_name, ns_frame_id_, EstimatorType_t::LATERAL, ch_, ph_, [this](int a, int b) { return this->getState(a, b); },
+      65      229380 :         [this](const Correction<lat_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t state) {
+      66      229380 :           return this->doCorrection(meas, R, state);
+      67             :         }));
+      68             :   }
+      69             : 
+      70             :   // | ------- check if all parameters loaded successfully ------ |
+      71         116 :   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         116 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      77             : 
+      78             :   // | ----------- initialize process noise covariance ---------- |
+      79         116 :   Q_ = Q_t::Zero();
+      80             :   double tmp_noise;
+      81         116 :   ph->param_loader->loadParam("process_noise/pos", tmp_noise);
+      82         116 :   Q_(stateIdToIndex(POSITION, AXIS_X), stateIdToIndex(POSITION, AXIS_X)) = tmp_noise;
+      83         116 :   Q_(stateIdToIndex(POSITION, AXIS_Y), stateIdToIndex(POSITION, AXIS_Y)) = tmp_noise;
+      84         116 :   ph->param_loader->loadParam("process_noise/vel", tmp_noise);
+      85         116 :   Q_(stateIdToIndex(VELOCITY, AXIS_X), stateIdToIndex(VELOCITY, AXIS_X)) = tmp_noise;
+      86         116 :   Q_(stateIdToIndex(VELOCITY, AXIS_Y), stateIdToIndex(VELOCITY, AXIS_Y)) = tmp_noise;
+      87         116 :   ph->param_loader->loadParam("process_noise/acc", tmp_noise);
+      88         116 :   Q_(stateIdToIndex(ACCELERATION, AXIS_X), stateIdToIndex(ACCELERATION, AXIS_X)) = tmp_noise;
+      89         116 :   Q_(stateIdToIndex(ACCELERATION, AXIS_Y), stateIdToIndex(ACCELERATION, AXIS_Y)) = tmp_noise;
+      90             : 
+      91             :   // | ------------- initialize dynamic reconfigure ------------- |
+      92             :   drmgr_ =
+      93         116 :       std::make_unique<drmgr_t>(ros::NodeHandle("~/" + getNamespacedName()), true, getPrintName(), boost::bind(&LatGeneric::callbackReconfigure, this, _1, _2));
+      94         116 :   drmgr_->config.pos = Q_(stateIdToIndex(POSITION, AXIS_X), stateIdToIndex(POSITION, AXIS_X));
+      95         116 :   drmgr_->config.vel = Q_(stateIdToIndex(VELOCITY, AXIS_X), stateIdToIndex(VELOCITY, AXIS_X));
+      96         116 :   drmgr_->config.acc = Q_(stateIdToIndex(ACCELERATION, AXIS_X), stateIdToIndex(ACCELERATION, AXIS_X));
+      97         116 :   drmgr_->update_config(drmgr_->config);
+      98             : 
+      99             :   // | --------------- Kalman filter intialization -------------- |
+     100         116 :   const x_t        x0 = x_t::Zero();
+     101         116 :   const P_t        P0 = 1e3 * P_t::Identity();
+     102         116 :   const statecov_t sc0({x0, P0});
+     103         116 :   sc_ = sc0;
+     104             : 
+     105         116 :   lkf_ = std::make_shared<lkf_t>(A_, B_, H_);
+     106         116 :   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         116 :   timer_update_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &LatGeneric::timerUpdate, this); 
+     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         232 :   mrs_lib::SubscribeHandlerOptions shopts;
+     124         116 :   shopts.nh                 = nh;
+     125         116 :   shopts.node_name          = getPrintName();
+     126         116 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     127         116 :   shopts.threadsafe         = true;
+     128         116 :   shopts.autostart          = true;
+     129         116 :   shopts.queue_size         = 10;
+     130         116 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     131             : 
+     132         116 :   sh_control_input_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput>(shopts, "control_input_in");
+     133             :   sh_hdg_state_ =
+     134         116 :       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         116 :   if (ch_->debug_topics.input) {
+     138         116 :     ph_input_ = mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped>(nh, getNamespacedName() + "/input", 10);
+     139             :   }
+     140         116 :   if (ch_->debug_topics.output) {
+     141         116 :     ph_output_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput>(nh, getNamespacedName() + "/output", 10);
+     142             :   }
+     143         116 :   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         116 :   if (changeState(INITIALIZED_STATE)) {
+     150         116 :     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         116 : }
+     155             : /*//}*/
+     156             : 
+     157             : /*//{ start() */
+     158        4330 : bool LatGeneric::start(void) {
+     159             : 
+     160        4330 :   if (isInState(READY_STATE)) {
+     161             :     /* timer_update_.start(); */
+     162         116 :     changeState(STARTED_STATE);
+     163         116 :     return true;
+     164             : 
+     165             :   } else {
+     166        4214 :     ROS_WARN_THROTTLE(1.0, "[%s]: Estimator must be in READY_STATE to start it", getPrintName().c_str());
+     167        4214 :     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 :   changeState(INITIALIZED_STATE);
+     211           0 :   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
+     212             : 
+     213           0 :   return true;
+     214             : }
+     215             : /*//}*/
+     216             : 
+     217             : /* timerUpdate() //{*/
+     218      222208 : void LatGeneric::timerUpdate(const ros::TimerEvent &event) {
+     219             : 
+     220      222208 :   if (!isInitialized()) {
+     221       23741 :     return;
+     222             :   }
+     223             : 
+     224      222203 :   switch (getCurrentSmState()) {
+     225             : 
+     226           0 :     case UNINITIALIZED_STATE: {
+     227           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s initialization", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     228           0 :       break;
+     229             :     }
+     230             : 
+     231          64 :     case READY_STATE: {
+     232          64 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s estimator start", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     233          64 :       break;
+     234             :     }
+     235             : 
+     236        7864 :     case INITIALIZED_STATE: {
+     237             :       // initialize the estimator with current corrections
+     238        8052 :       for (auto correction : corrections_) {
+     239        7936 :         auto res = correction->getProcessedCorrection();
+     240        7936 :         if (res) {
+     241         188 :           auto measurement_stamped = res.value(); 
+     242         188 :           setState(measurement_stamped.value(AXIS_X), correction->getStateId(), AXIS_X);
+     243         188 :           setState(measurement_stamped.value(AXIS_Y), correction->getStateId(), AXIS_Y);
+     244         188 :           ROS_INFO_THROTTLE(1.0, "[%s]: Setting initial state to: %.2f %.2f", getPrintName().c_str(), measurement_stamped.value(AXIS_X),
+     245             :                             measurement_stamped.value(AXIS_Y));
+     246             :         } else {
+     247        7748 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), correction->getNamespacedName().c_str());
+     248        7748 :           return;
+     249             :         }
+     250             :       }
+     251         116 :       ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     252         116 :       changeState(READY_STATE);
+     253         116 :       break;
+     254             :     }
+     255             : 
+     256         116 :     case STARTED_STATE: {
+     257         116 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     258         116 :       if (isConverged()) {
+     259         116 :         ROS_INFO_THROTTLE(1.0, "[%s]: LKF converged", getPrintName().c_str());
+     260         116 :         changeState(RUNNING_STATE);
+     261             :       }
+     262         116 :       break;
+     263             :     }
+     264             : 
+     265      214157 :     case RUNNING_STATE: {
+     266      439535 :       for (auto correction : corrections_) {
+     267      225375 :         if (!correction->isHealthy()) {
+     268           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     269           0 :           changeState(ERROR_STATE);
+     270             :         }
+     271             :       }
+     272      214160 :       break;
+     273             :     }
+     274             : 
+     275           0 :     case STOPPED_STATE: {
+     276           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is stopped", getPrintName().c_str());
+     277           0 :       break;
+     278             :     }
+     279             : 
+     280           0 :     case ERROR_STATE: {
+     281           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is in ERROR state", getPrintName().c_str());
+     282             : 
+     283           0 :       ros::Time t_now = ros::Time::now();
+     284           0 :       if (is_error_state_first_time_) {
+     285           0 :         prev_time_in_error_state_  = t_now;
+     286           0 :         is_error_state_first_time_ = false;
+     287           0 :         error_state_duration_      = ros::Duration(0.0);
+     288             :       }
+     289           0 :       error_state_duration_ += t_now - prev_time_in_error_state_;
+     290             : 
+     291             : 
+     292             :       // check if all corrections are healthy now
+     293           0 :       bool all_corrections_healthy = true;
+     294           0 :       for (auto correction : corrections_) {
+     295           0 :         if (!correction->isHealthy()) {
+     296           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     297           0 :           all_corrections_healthy = false;
+     298             :         }
+     299             :       }
+     300             : 
+     301           0 :       if (all_corrections_healthy && innovation_ok_) {
+     302             :         // initialize the estimator again if corrections become healthy
+     303           0 :         if (error_state_duration_.toSec() > 5.0) {
+     304           0 :           ROS_INFO("[%s]: corrections healthy for %.2f s", getPrintName().c_str(), error_state_duration_.toSec());
+     305           0 :           changeState(INITIALIZED_STATE);
+     306           0 :           is_error_state_first_time_ = true;
+     307           0 :         }
+     308             :       } else {
+     309           0 :         is_error_state_first_time_ = true;
+     310             :       }
+     311             : 
+     312           0 :       prev_time_in_error_state_ = t_now;
+     313             : 
+     314           0 :       break;
+     315             :     }
+     316             :   }
+     317             : 
+     318      214459 :   if (sh_control_input_.newMsg()) {
+     319      107902 :     is_input_ready_ = true;
+     320             :   }
+     321             : 
+     322             :   // check age of input
+     323      214454 :   if (is_input_ready_ && (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {  // TODO: parametrize, if older than say 1 second, eland
+     324          16 :     ROS_WARN_THROTTLE(1.0, "[%s]: input too old (%.4f s), using zero input instead", getPrintName().c_str(),
+     325             :                       (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec());
+     326          16 :     is_input_ready_ = false;
+     327             :   }
+     328             : 
+     329      214450 :   if (fun_get_hdg_()) {
+     330      214230 :     is_hdg_state_ready_ = true;
+     331             :   }
+     332             : 
+     333      214456 :   if (!isRunning() && !isStarted()) {
+     334         161 :     return;
+     335             :   }
+     336             : 
+     337      214286 :   if (first_iter_) {
+     338         116 :     first_iter_ = false;
+     339         116 :     return;
+     340             :   }
+     341             : 
+     342             :   // obtain dt for state prediction
+     343      214170 :   double dt = (event.current_real - event.last_real).toSec();
+     344      214166 :   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
+     345       15710 :     return;
+     346             :   }
+     347             : 
+     348      198456 :   if (!is_repredictor_enabled_) { // repredictor calculates dt on its own
+     349      198457 :     setDt(dt);
+     350             :   }
+     351             : 
+     352             :   // obtain unbiased desired control acceleration in the estimator frame that will be used as input to the estimator
+     353      198461 :   u_t       u;
+     354      198442 :   ros::Time input_stamp;
+     355      198454 :   if (is_input_ready_ && is_hdg_state_ready_) {
+     356      121865 :     auto res = fun_get_hdg_();
+     357      121872 :     if (!res) {
+     358           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: could not obtain heading", getPrintName().c_str());
+     359           0 :       return;
+     360             :     }
+     361             : 
+     362      121870 :     const tf2::Vector3 des_acc_global = getAccGlobal(sh_control_input_.getMsg(), res.value());
+     363      121782 :     input_stamp                       = sh_control_input_.getMsg()->header.stamp;
+     364      121831 :     if (input_coeff_ != default_input_coeff_){
+     365          93 :       setInputCoeff(default_input_coeff_);
+     366             :     }
+     367      121831 :     u(0) = des_acc_global.getX();
+     368      121750 :     u(1) = des_acc_global.getY();
+     369             : 
+     370             :   } else {  // this is ok before the controller starts controlling but bad during actual flight (causes delayed estimated acceleration and velocity)
+     371       76580 :     ROS_DEBUG_THROTTLE(1.0, "[%s]: not receiving control input, estimation suboptimal, potentially unstable", getPrintName().c_str());
+     372       76583 :     input_stamp = ros::Time::now();
+     373       76584 :     if (input_coeff_ != 0){
+     374         132 :       setInputCoeff(0);
+     375             :     }
+     376       76584 :     u = u_t::Zero();
+     377             :   }
+     378             : 
+     379             :   // go through available corrections and apply them
+     380             :   /* for (auto correction : corrections_) { */
+     381             :   /*   auto res = correction->getProcessedCorrection(); */
+     382             :   /*   if (res) { */
+     383             :   /*     auto measurement_stamped = res.value(); */
+     384             :   /*     doCorrection(measurement_stamped.value, correction->getR(), correction->getStateId(), measurement_stamped.stamp); */
+     385             :   /*   } */
+     386             :   /* } */
+     387             : 
+     388             :   // get current state, covariance, and process noise
+     389      198413 :   statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+     390      198427 :   Q_t        Q  = mrs_lib::get_mutexed(mtx_Q_, Q_);
+     391             : 
+     392             :   // prediction step
+     393             :   try {
+     394             :     // Apply the prediction step
+     395      396883 :     std::scoped_lock lock(mutex_lkf_);
+     396      198426 :     if (is_repredictor_enabled_) {
+     397           0 :       lkf_rep_->addInputChangeWithNoise(u, Q, input_stamp, models_[0]);
+     398           0 :       sc = lkf_rep_->predictTo(ros::Time::now());
+     399             :     } else {
+     400      198426 :       sc = lkf_->predict(sc, u, Q, dt_);
+     401             :     }
+     402             :   }
+     403           0 :   catch (const std::exception &e) {
+     404           0 :     ROS_ERROR("[%s]: LKF prediction failed: %s", getPrintName().c_str(), e.what());
+     405           0 :     changeState(ERROR_STATE);
+     406             :   }
+     407             : 
+     408             :   // update the state and covariance variable that is queried by the estimation manager
+     409      198407 :   mrs_lib::set_mutexed(mutex_sc_, sc, sc_);
+     410             : 
+     411             :   // publishing
+     412      198421 :   publishInput(u, input_stamp);
+     413      198463 :   publishOutput();
+     414      198464 :   publishDiagnostics();
+     415             : }
+     416             : /*//}*/
+     417             : 
+     418             : /*//{ timerCheckHealth() */
+     419           0 : void LatGeneric::timerCheckHealth([[maybe_unused]] const ros::TimerEvent &event) {
+     420             : 
+     421           0 :   if (!isInitialized()) {
+     422           0 :     return;
+     423             :   }
+     424             : 
+     425           0 :   switch (getCurrentSmState()) {
+     426             : 
+     427           0 :     case UNINITIALIZED_STATE: {
+     428           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s initialization", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     429           0 :       break;
+     430             :     }
+     431             : 
+     432           0 :     case READY_STATE: {
+     433           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s estimator start", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     434           0 :       break;
+     435             :     }
+     436             : 
+     437           0 :     case INITIALIZED_STATE: {
+     438             :       // initialize the estimator with current corrections
+     439           0 :       for (auto correction : corrections_) {
+     440           0 :         auto res = correction->getProcessedCorrection();
+     441           0 :         if (res) {
+     442           0 :           auto measurement_stamped = res.value();
+     443           0 :           setState(measurement_stamped.value(AXIS_X), correction->getStateId(), AXIS_X);
+     444           0 :           setState(measurement_stamped.value(AXIS_Y), correction->getStateId(), AXIS_Y);
+     445           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: Setting initial state to: %.2f %.2f", getPrintName().c_str(), measurement_stamped.value(AXIS_X),
+     446             :                             measurement_stamped.value(AXIS_Y));
+     447             :         } else {
+     448           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), correction->getPrintName().c_str());
+     449           0 :           return;
+     450             :         }
+     451             :       }
+     452           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     453           0 :       changeState(READY_STATE);
+     454           0 :       break;
+     455             :     }
+     456             : 
+     457           0 :     case STARTED_STATE: {
+     458           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     459           0 :       if (isConverged()) {
+     460           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: LKF converged", getPrintName().c_str());
+     461           0 :         changeState(RUNNING_STATE);
+     462             :       }
+     463           0 :       break;
+     464             :     }
+     465             : 
+     466           0 :     case RUNNING_STATE: {
+     467           0 :       for (auto correction : corrections_) {
+     468           0 :         if (!correction->isHealthy()) {
+     469           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     470           0 :           changeState(ERROR_STATE);
+     471             :         }
+     472             :       }
+     473           0 :       break;
+     474             :     }
+     475             : 
+     476           0 :     case STOPPED_STATE: {
+     477           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is stopped", getPrintName().c_str());
+     478           0 :       break;
+     479             :     }
+     480             : 
+     481           0 :     case ERROR_STATE: {
+     482           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is in ERROR state", getPrintName().c_str());
+     483           0 :       bool all_corrections_healthy = true;
+     484           0 :       for (auto correction : corrections_) {
+     485           0 :         if (!correction->isHealthy()) {
+     486           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     487           0 :           correction->resetProcessors();
+     488           0 :           all_corrections_healthy = false;
+     489             :         }
+     490             :       }
+     491             :       // initialize the estimator again if corrections become healthy
+     492           0 :       if (all_corrections_healthy) {
+     493           0 :         changeState(INITIALIZED_STATE);
+     494             :       }
+     495           0 :       break;
+     496             :     }
+     497             :   }
+     498             : 
+     499           0 :   if (sh_control_input_.newMsg()) {
+     500           0 :     is_input_ready_ = true;
+     501             :   }
+     502             : 
+     503             :   // check age of input
+     504           0 :   if (is_input_ready_ && (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {  // TODO: parametrize, if older than say 1 second, eland
+     505           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: input too old (%.4f s), using zero input instead", getPrintName().c_str(),
+     506             :                       (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec());
+     507           0 :     is_input_ready_ = false;
+     508             :   }
+     509             : 
+     510           0 :   if (fun_get_hdg_()) {
+     511           0 :     is_hdg_state_ready_ = true;
+     512             :   }
+     513             : }
+     514             : /*//}*/
+     515             : 
+     516             : /*//{ doCorrection() */
+     517      229399 : void LatGeneric::doCorrection(const Correction<lat_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t &state_id) {
+     518      229399 :   doCorrection(meas.value, R, state_id, meas.stamp);
+     519      229375 : }
+     520             : /*//}*/
+     521             : 
+     522             : /*//{ doCorrection() */
+     523      229246 : void LatGeneric::doCorrection(const z_t &z, const double R, const StateId_t &state_id, const ros::Time &meas_stamp) {
+     524             : 
+     525      229246 :   if (!isInitialized()) {
+     526         183 :     return;
+     527             :   }
+     528             : 
+     529             :   // we do not want to perform corrections until the estimator is initialized
+     530      229238 :   if (!(isInState(SMStates_t::READY_STATE) || isInState(SMStates_t::RUNNING_STATE) || isInState(SMStates_t::STARTED_STATE))) {
+     531         182 :     return;
+     532             :   }
+     533             : 
+     534             :   // for position state check the innovation
+     535      228929 :   if (state_id == POSITION) {
+     536             :     {
+     537      218004 :       std::scoped_lock lock(mtx_innovation_);
+     538             : 
+     539      218090 :       is_mitigating_jump_ = false;
+     540      218192 :       innovation_(0)      = z(0) - getState(POSITION, AXIS_X);
+     541      218015 :       innovation_(1)      = z(1) - getState(POSITION, AXIS_Y);
+     542             : 
+     543      218109 :       if (fabs(innovation_(0)) > pos_innovation_limit_ || fabs(innovation_(1)) > pos_innovation_limit_) {
+     544           0 :         ROS_WARN_THROTTLE(1.0, "[%s]: innovation too large - [%.2f %.2f] lim: %.2f", getPrintName().c_str(), innovation_(0), innovation_(1),
+     545             :                           pos_innovation_limit_);
+     546           0 :         innovation_ok_ = false;
+     547           0 :         switch (exc_innovation_action_) {
+     548           0 :           case ExcInnoAction_t::ELAND: {
+     549           0 :             ROS_WARN_THROTTLE(1.0, "[%s]: large innovation should trigger eland in control manager", getPrintName().c_str());
+     550           0 :             changeState(ERROR_STATE);
+     551           0 :             break;
+     552             :           }
+     553           0 :           case ExcInnoAction_t::SWITCH: {
+     554           0 :             ROS_WARN_THROTTLE(1.0, "[%s]: innovation should trigger estimator switch but no eland", getPrintName().c_str());
+     555           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?
+     556           0 :             innovation_(1) = 0.0;
+     557           0 :             changeState(ERROR_STATE);
+     558           0 :             break;
+     559             :           }
+     560           0 :           case ExcInnoAction_t::MITIGATE: {
+     561           0 :             ROS_WARN_THROTTLE(1.0, "[%s]: large innovation should trigger estimate jump mitigation", getPrintName().c_str());
+     562           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?
+     563           0 :             innovation_(1)      = 0.0;
+     564           0 :             is_mitigating_jump_ = true;
+     565           0 :             setState(z(0), POSITION, AXIS_X);
+     566           0 :             setState(z(1), POSITION, AXIS_Y);
+     567           0 :             break;
+     568             :           }
+     569           0 :           case ExcInnoAction_t::NONE: {
+     570           0 :             ROS_WARN_THROTTLE(1.0, "[%s]: large innovation ignored", getPrintName().c_str());
+     571           0 :             break;
+     572             :           }
+     573             :         }
+     574             :       }
+     575      218129 :       innovation_ok_ = true;
+     576             :     }
+     577             :   }
+     578             : 
+     579      229021 :   statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+     580             :   try {
+     581             :     // Apply the correction step
+     582      458140 :     std::scoped_lock lock(mutex_lkf_);
+     583      229101 :     H_                           = H_t::Zero();
+     584      229066 :     H_(AXIS_X, state_id * 2)     = 1;
+     585      229024 :     H_(AXIS_Y, state_id * 2 + 1) = 1;
+     586      229094 :     lkf_->H                      = H_;
+     587             : 
+     588      228599 :     if (is_repredictor_enabled_) {
+     589             : 
+     590           0 :       lkf_rep_->addMeasurement(z, R_t::Ones() * R, meas_stamp, models_[state_id]);
+     591             :     } else {
+     592      228599 :       sc = lkf_->correct(sc, z, R_t::Ones() * R);
+     593             :     }
+     594             :   }
+     595           0 :   catch (const std::exception &e) {
+     596             :     // In case of error, alert the user
+     597           0 :     ROS_ERROR("[%s]: LKF correction failed: %s", getPrintName().c_str(), e.what());
+     598             :   }
+     599             : 
+     600      228711 :   mrs_lib::set_mutexed(mutex_sc_, sc, sc_);
+     601             : }  // namespace mrs_uav_state_estimators
+     602             : /*//}*/
+     603             : 
+     604             : /*//{ isConverged() */
+     605         116 : bool LatGeneric::isConverged() {
+     606             : 
+     607             :   // TODO: check convergence by rate of change of determinant
+     608             : 
+     609         116 :   return true;
+     610             : }
+     611             : /*//}*/
+     612             : 
+     613             : /*//{ getState() */
+     614     1705391 : double LatGeneric::getState(const int &state_id_in, const int &axis_in) const {
+     615     1705391 :   return getState(stateIdToIndex(state_id_in, axis_in));
+     616             : }
+     617             : 
+     618     1705029 : double LatGeneric::getState(const int &state_idx_in) const {
+     619     1705029 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).x(state_idx_in);
+     620             : }
+     621             : /*//}*/
+     622             : 
+     623             : /*//{ setState() */
+     624         376 : void LatGeneric::setState(const double &state_in, const int &state_id_in, const int &axis_in) {
+     625         376 :   setState(state_in, stateIdToIndex(state_id_in, axis_in));
+     626         376 : }
+     627             : 
+     628         376 : void LatGeneric::setState(const double &state_in, const int &state_idx_in) {
+     629         376 :   mrs_lib::set_mutexed(mutex_sc_, state_in, sc_.x(state_idx_in));
+     630         376 : }
+     631             : /*//}*/
+     632             : 
+     633             : /*//{ getStates() */
+     634      198463 : LatGeneric::states_t LatGeneric::getStates(void) const {
+     635      396914 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).x;
+     636             : }
+     637             : /*//}*/
+     638             : 
+     639             : /*//{ setStates() */
+     640           0 : void LatGeneric::setStates(const states_t &states_in) {
+     641           0 :   mrs_lib::set_mutexed(mutex_sc_, states_in, sc_.x);
+     642           0 : }
+     643             : /*//}*/
+     644             : 
+     645             : /*//{ getCovariance() */
+     646      840863 : double LatGeneric::getCovariance(const int &state_id_in, const int &axis_in) const {
+     647      840863 :   return getCovariance(stateIdToIndex(state_id_in, axis_in));
+     648             : }
+     649             : 
+     650      840840 : double LatGeneric::getCovariance(const int &state_idx_in) const {
+     651      840840 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).P(state_idx_in, state_idx_in);
+     652             : }
+     653             : /*//}*/
+     654             : 
+     655             : /*//{ getCovarianceMatrix() */
+     656      198457 : LatGeneric::covariance_t LatGeneric::getCovarianceMatrix(void) const {
+     657      396905 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).P;
+     658             : }
+     659             : /*//}*/
+     660             : 
+     661             : /*//{ setCovarianceMatrix() */
+     662           0 : void LatGeneric::setCovarianceMatrix(const covariance_t &cov_in) {
+     663           0 :   mrs_lib::set_mutexed(mutex_sc_, cov_in, sc_.P);
+     664           0 : }
+     665             : /*//}*/
+     666             : 
+     667             : /*//{ getInnovation() */
+     668      420407 : double LatGeneric::getInnovation(const int &state_idx) const {
+     669      420407 :   return mrs_lib::get_mutexed(mtx_innovation_, innovation_)(state_idx);
+     670             : }
+     671             : 
+     672      420424 : double LatGeneric::getInnovation(const int &state_id_in, const int &axis_in) const {
+     673      420424 :   return getInnovation(stateIdToIndex(state_id_in, axis_in));
+     674             : }
+     675             : /*//}*/
+     676             : 
+     677             : /*//{ setDt() */
+     678      198459 : void LatGeneric::setDt(const double &dt) {
+     679      198459 :   dt_ = dt;
+     680      198459 :   generateA();
+     681      198462 :   generateB();
+     682      396915 :   std::scoped_lock lock(mutex_lkf_);
+     683      198458 :   lkf_->A = A_;
+     684      198460 :   lkf_->B = B_;
+     685      198455 : }
+     686             : /*//}*/
+     687             : 
+     688             : /*//{ setInputCoeff() */
+     689         225 : void LatGeneric::setInputCoeff(const double &input_coeff) {
+     690         225 :   input_coeff_ = input_coeff;
+     691         225 :   generateA();
+     692         225 :   generateB();
+     693         450 :   std::scoped_lock lock(mutex_lkf_);
+     694         225 :   lkf_->A = A_;
+     695         225 :   lkf_->B = B_;
+     696             : 
+     697         225 :   if (is_repredictor_enabled_) {
+     698           0 :     models_.clear();
+     699           0 :     generateRepredictorModels(input_coeff_);
+     700             :   }
+     701         225 : }
+     702             : /*//}*/
+     703             : 
+     704             : /*//{ generateRepredictorModels() */
+     705           0 : void LatGeneric::generateRepredictorModels(const double input_coeff) {
+     706           0 :     for (int i = 0; (int)(i < lat_generic::n_states / 2); i++) {
+     707             : 
+     708           0 :       auto lambda_generateA = [input_coeff](const double dt) {
+     709           0 :         A_t A;
+     710             :         // clang-format off
+     711           0 :         A <<
+     712           0 :           1, 0, dt, 0, 0.5 * dt * dt, 0,
+     713           0 :           0, 1, 0, dt, 0, 0.5 * dt * dt,
+     714           0 :           0, 0, 1, 0, dt, 0,
+     715           0 :           0, 0, 0, 1, 0, dt,
+     716           0 :           0, 0, 0, 0, 1-(input_coeff * dt), 0,
+     717           0 :           0, 0, 0, 0, 0, 1-(input_coeff * dt);
+     718             :         // clang-format on
+     719           0 :         return A;
+     720           0 :       };
+     721             : 
+     722           0 :       auto lambda_generateB = [input_coeff]([[maybe_unused]] const double dt) {
+     723           0 :         B_t B = B.Zero();
+     724             :         // clang-format off
+     725           0 :           B <<
+     726           0 :             0, 0,
+     727           0 :             0, 0,
+     728           0 :             0, 0,
+     729           0 :             0, 0,
+     730           0 :             input_coeff * dt, 0,
+     731           0 :             0, input_coeff * dt;
+     732             :         // clang-format on
+     733             : 
+     734           0 :         return B;
+     735           0 :       };
+     736             : 
+     737           0 :       H_t H                = H.Zero();
+     738           0 :       H(AXIS_X, i * 2)     = 1;
+     739           0 :       H(AXIS_Y, i * 2 + 1) = 1;
+     740           0 :       models_.push_back(std::make_shared<varstep_lkf_t>(lambda_generateA, lambda_generateB, H));
+     741             :     }
+     742           0 : }
+     743             : /*//}*/
+     744             : 
+     745             : /*//{ generateA() */
+     746      198801 : void LatGeneric::generateA() {
+     747             : 
+     748             :   // clang-format off
+     749      198801 :     A_ <<
+     750      198796 :       1, 0, dt_, 0, 0.5 * dt_ * dt_, 0,
+     751      198802 :       0, 1, 0, dt_, 0, 0.5 * dt_ * dt_,
+     752      198801 :       0, 0, 1, 0, dt_, 0,
+     753      198803 :       0, 0, 0, 1, 0, dt_,
+     754      198802 :       0, 0, 0, 0, 1-(input_coeff_ * dt_), 0,
+     755      198803 :       0, 0, 0, 0, 0, 1-(input_coeff_ * dt_);
+     756             :   // clang-format on
+     757      198801 : }
+     758             : /*//}*/
+     759             : 
+     760             : /*//{ generateB() */
+     761      198804 : void LatGeneric::generateB() {
+     762             : 
+     763             :   // clang-format off
+     764      198804 :     B_ <<
+     765      198804 :       0, 0,
+     766      198788 :       0, 0,
+     767      198793 :       0, 0,
+     768      198796 :       0, 0,
+     769      198802 :       input_coeff_ * dt_, 0,
+     770      198799 :       0, input_coeff_ * dt_;
+     771             :   // clang-format on
+     772      198795 : }
+     773             : /*//}*/
+     774             : 
+     775             : /*//{ callbackReconfigure() */
+     776         116 : void LatGeneric::callbackReconfigure(LateralEstimatorConfig &config, [[maybe_unused]] uint32_t level) {
+     777             : 
+     778         116 :   if (!isInitialized()) {
+     779         116 :     return;
+     780             :   }
+     781             : 
+     782           0 :   Q_t Q;
+     783           0 :   Q(stateIdToIndex(POSITION, AXIS_X), stateIdToIndex(POSITION, AXIS_X))         = config.pos;
+     784           0 :   Q(stateIdToIndex(POSITION, AXIS_Y), stateIdToIndex(POSITION, AXIS_Y))         = config.pos;
+     785           0 :   Q(stateIdToIndex(VELOCITY, AXIS_X), stateIdToIndex(VELOCITY, AXIS_X))         = config.vel;
+     786           0 :   Q(stateIdToIndex(VELOCITY, AXIS_Y), stateIdToIndex(VELOCITY, AXIS_Y))         = config.vel;
+     787           0 :   Q(stateIdToIndex(ACCELERATION, AXIS_X), stateIdToIndex(ACCELERATION, AXIS_X)) = config.acc;
+     788           0 :   Q(stateIdToIndex(ACCELERATION, AXIS_Y), stateIdToIndex(ACCELERATION, AXIS_Y)) = config.acc;
+     789           0 :   mrs_lib::set_mutexed(mtx_Q_, Q, Q_);
+     790             : }
+     791             : /*//}*/
+     792             : 
+     793             : /*//{ getNamespacedName() */
+     794         700 : std::string LatGeneric::getNamespacedName() const {
+     795        1400 :   return parent_state_est_name_ + "/" + getName();
+     796             : }
+     797             : /*//}*/
+     798             : 
+     799             : /*//{ getPrintName() */
+     800        1084 : std::string LatGeneric::getPrintName() const {
+     801        2168 :   return ch_->nodelet_name + "/" + parent_state_est_name_ + "/" + getName();
+     802             : }
+     803             : /*//}*/
+     804             : };  // 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..c2c3e8a4a4 --- /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..3faff34772051873b669e5741ac9ddc8170c0df3 GIT binary patch literal 3153 zcmV-X46gHuP)_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&xCF;4X|g)?9qevDq?rJB$a|B7zxn8$HlnW?+~NY6V{jt zb;YwTbku;kVh9~HJjO6N7qYj2t{+6^G9cg+3P7b^*bb7q@b3er03`kOiCwTcv9N#E{Z?arkr1=dx)3f3HzT{98?+hF*9gGc1 z-^ybHsN_hE{P{Hux7!{=W-=K0G~F%okTS? zplNF;AA>B=Kxs#SreWXfDMkUM6=zNbu4zO2m{rMy7)aOd}r+sk8TOPmm!A z=3f|oz-bvc z%AWqr16nw=g$5}bO1hkG>`(98s|#Y12Z9P>dISFj5qq>{hc)Y@N`M#-WHA94S55KJ z*G%8i#SR?L0EZfU7-RVHkFp0*-HX`KHmlLcb!&89UFv5OL$XWVuCcq*pCx9B9d5U% zI!cl;q{EO*Q}HIv?(Wzx;3+REvO>-Q)Smw?FX=HwL*&)%K~j78wC;BPI5N5Z~rh7$Y4#sZvIdVgE?h zq@-R;!z{75+9D~#RWAh{=qfP-4A*;w0q_cw0ymK%;G5e55Izt}^Eg)~x=+XU&85`} zMg{%?oFya`;BGukMNkD}?`0jFrPz?%2P$E6U1Vni-N_h{t}%NJ z(S(;8zy|V$E38MR? zZk#{UFz_#Rg;F1(4ba~}j7|?Ed?AesbkuyD> zF?$biD@Ri9e2;HcI@6*pYdnUjVSt@FABtQ)yx)4co?Tbqn6o#Eu-7`HHWz<`&J0&c zQ{iJL0z|4Qnv~b&f~}}y)#=yP6_on*NOjKy_)&&`Nzv{H8wS>2vNM(3KK~aD1HZ1? z{1n*#;j%R9v`>$SD0gXRh!7g?2-dW1+Pk55_^Nds^230$f5#5%SKSsNxWyeU12941SHDX2!-`@kQ!z3}1C< zuQ(PMqtKm60ZELkb+C1>e@@Nf@O4c{TI?;Le1R?jN>mU)Q%18TMxk2R8*s$S&H&5z z!&6V*dU`5B_-X;wHGU0ySpHb|x}tgwg`zcxbmmK96$_|SotLM$*8o_8LGFs^Fd)0K z8t^@aYcZF?@QmYvH6Fw2bB*5daq1HwE+_+<$4Ir=>YH6q@+EIbPcbZyyoqP#0jRYM zX27H6O9ezflb#nn^u%3T{o0Iit>W-K8;lLKCh5$(_3Pw?FxLh@S-;LlQ0@zpObXZ% zz0iI6vp&VLv?t+{tP5Ahg}rv1hfQurYwWiYcI3J~L>^z?VIzotIyBMa z7InJW9du@ugyc#!iz1%Rq{Cg9L;z~kz3=y=SqcfK432e5CJW#v;%&D zJ7Z8T#yT?BA*;{l(n=bCX$!lH1B}Ml6_N!VL(2Cwa)7_)(Y%IJPU_|^RfBZMO|cng z+d_Hdi z;lYU=KnWKE$m3!F)0S-<>I!3FI}cd+D9;SFI7|1ef!$|FPZ8hP2f8Qei4eVZJiGK* z#3z?DOzinEVA13*#w=kfph + + + + + + 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-12-17 22:35:29Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()30
mrs_uav_state_estimators::gps_baro::GpsBaro::GpsBaro()30
mrs_uav_state_estimators::gps_baro::GpsBaro::~GpsBaro()30
mrs_uav_state_estimators::gps_baro::GpsBaro::~GpsBaro().230
+
+
+ + + +
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..d2256059f7 --- /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-12-17 22:35:29Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()30
mrs_uav_state_estimators::gps_baro::GpsBaro::GpsBaro()30
mrs_uav_state_estimators::gps_baro::GpsBaro::~GpsBaro()30
mrs_uav_state_estimators::gps_baro::GpsBaro::~GpsBaro().230
+
+
+ + + +
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..66209a4d51 --- /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-12-17 22:35:29Functions: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          30 :   GpsBaro() : StateGeneric(estimator_name, is_core_plugin) {
+      15          30 :   }
+      16             : 
+      17          60 :   ~GpsBaro(void) {
+      18          60 :   }
+      19             : };
+      20             : 
+      21             : }  // namespace gps_baro
+      22             : }  // namespace mrs_uav_state_estimators
+      23             : 
+      24             : #include <pluginlib/class_list_macros.h>
+      25          30 : 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-12-17 22:35:29Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()82
mrs_uav_state_estimators::gps_garmin::GpsGarmin::GpsGarmin()82
mrs_uav_state_estimators::gps_garmin::GpsGarmin::~GpsGarmin()82
mrs_uav_state_estimators::gps_garmin::GpsGarmin::~GpsGarmin().282
+
+
+ + + +
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..439a7fb638 --- /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-12-17 22:35:29Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()82
mrs_uav_state_estimators::gps_garmin::GpsGarmin::GpsGarmin()82
mrs_uav_state_estimators::gps_garmin::GpsGarmin::~GpsGarmin()82
mrs_uav_state_estimators::gps_garmin::GpsGarmin::~GpsGarmin().282
+
+
+ + + +
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..454e78b20f --- /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-12-17 22:35:29Functions: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          82 :   GpsGarmin() : StateGeneric(estimator_name, is_core_plugin) {
+      15          82 :   }
+      16             : 
+      17         164 :   ~GpsGarmin(void) {
+      18         164 :   }
+      19             : };
+      20             : 
+      21             : }  // namespace gps_garmin
+      22             : }  // namespace mrs_uav_state_estimators
+      23             : 
+      24             : #include <pluginlib/class_list_macros.h>
+      25          82 : 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..4bab2c07fc --- /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:32650564.6 %
Date:2024-12-17 22:35:29Functions:283873.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 +
58.7%58.7%
+ 		58.7 %111 / 18950.0 %5 / 10
<unnamed>58.7 %111 / 18950.0 %5 / 10
state_generic.cpp +
65.9%65.9%
+ 		65.9 %195 / 29658.3 %7 / 12
<unnamed>65.9 %195 / 29658.3 %7 / 12
rtk_garmin.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
gps_baro.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..ea4eacddc4 --- /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:32650564.6 %
Date:2024-12-17 22:35:29Functions:283873.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 +
58.7%58.7%
+ 		58.7 %111 / 18950.0 %5 / 10
<unnamed>58.7 %111 / 18950.0 %5 / 10
state_generic.cpp +
65.9%65.9%
+ 		65.9 %195 / 29658.3 %7 / 12
<unnamed>65.9 %195 / 29658.3 %7 / 12
rtk_garmin.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
gps_baro.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..9fa3b1ba0e --- /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:32650564.6 %
Date:2024-12-17 22:35:29Functions:283873.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 +
58.7%58.7%
+ 		58.7 %111 / 18950.0 %5 / 10
<unnamed>58.7 %111 / 18950.0 %5 / 10
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 +
65.9%65.9%
+ 		65.9 %195 / 29658.3 %7 / 12
<unnamed>65.9 %195 / 29658.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..495fa5f7f0 --- /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:32650564.6 %
Date:2024-12-17 22:35:29Functions:283873.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 +
58.7%58.7%
+ 		58.7 %111 / 18950.0 %5 / 10
state_generic.cpp +
65.9%65.9%
+ 		65.9 %195 / 29658.3 %7 / 12
rtk_garmin.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
gps_baro.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..e35b0858e6 --- /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:32650564.6 %
Date:2024-12-17 22:35:29Functions:283873.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 +
58.7%58.7%
+ 		58.7 %111 / 18950.0 %5 / 10
state_generic.cpp +
65.9%65.9%
+ 		65.9 %195 / 29658.3 %7 / 12
rtk_garmin.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
gps_baro.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..853dbb5337 --- /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:32650564.6 %
Date:2024-12-17 22:35:29Functions:283873.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 +
58.7%58.7%
+ 		58.7 %111 / 18950.0 %5 / 10
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 +
65.9%65.9%
+ 		65.9 %195 / 29658.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..c2afa18820 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.func-sort-c.html @@ -0,0 +1,120 @@ + + + + + + + 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:11118958.7 %
Date:2024-12-17 22:35:29Functions:51050.0 %
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::timerUpdate(ros::TimerEvent 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::timerCheckPassthroughOdomHz(ros::TimerEvent const&)3
mrs_uav_state_estimators::passthrough::Passthrough::callbackPassthroughOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)1317
+
+
+ + + +
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..713f5c3e56 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.func.html @@ -0,0 +1,120 @@ + + + + + + + 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:11118958.7 %
Date:2024-12-17 22:35:29Functions:51050.0 %
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&)0
mrs_uav_state_estimators::passthrough::Passthrough::callbackPassthroughOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)1317
mrs_uav_state_estimators::passthrough::Passthrough::timerCheckPassthroughOdomHz(ros::TimerEvent const&)3
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..c7df3eb5b0 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.html @@ -0,0 +1,452 @@ + + + + + + + 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:11118958.7 %
Date:2024-12-17 22:35:29Functions:51050.0 %
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 &parent_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           2 :   ros::NodeHandle nh(parent_nh);
+      20             : 
+      21           1 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      22             : 
+      23             :   // | --------------- param loader initialization -------------- |
+      24             : 
+      25           1 :   if (is_core_plugin_) {
+      26             : 
+      27           1 :     ph_->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml");
+      28           1 :     ph_->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml");
+      29             :   }
+      30             : 
+      31           1 :   ph_->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getName() + "/");
+      32             : 
+      33             :   // | --------------------- load parameters -------------------- |
+      34           1 :   ph_->param_loader->loadParam("max_flight_z", max_flight_z_);
+      35           1 :   ph_->param_loader->loadParam("message/topic", msg_topic_);
+      36           1 :   ph_->param_loader->loadParam("kickoff", kickoff_, false);
+      37           1 :   msg_topic_ = "/" + ch_->uav_name + "/" + msg_topic_;
+      38             : 
+      39             :   // | --------------- subscribers initialization --------------- |
+      40           2 :   mrs_lib::SubscribeHandlerOptions shopts;
+      41           1 :   shopts.nh                 = nh;
+      42           1 :   shopts.node_name          = getPrintName();
+      43           1 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      44           1 :   shopts.threadsafe         = true;
+      45           1 :   shopts.autostart          = true;
+      46           1 :   shopts.queue_size         = 10;
+      47           1 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      48             : 
+      49           1 :   sh_passthrough_odom_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, msg_topic_, &Passthrough::callbackPassthroughOdom, this);
+      50             : 
+      51             :   // | ------------------ timers initialization ----------------- |
+      52           1 :   t_check_hz_last_                 = ros::WallTime::now();
+      53           1 :   prev_avg_hz_                     = 0;
+      54           1 :   timer_check_passthrough_odom_hz_ = nh.createTimer(ros::Rate(1.0), &Passthrough::timerCheckPassthroughOdomHz, this);
+      55             : 
+      56             :   // | ---------------- publishers initialization --------------- |
+      57           1 :   ph_odom_ = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh, Support::toSnakeCase(getName()) + "/odom", 10);  // needed for tf
+      58           1 :   if (ch_->debug_topics.state) {
+      59           1 :     ph_uav_state_ = mrs_lib::PublisherHandler<mrs_msgs::UavState>(nh, Support::toSnakeCase(getName()) + "/uav_state", 10);
+      60             :   }
+      61           1 :   if (ch_->debug_topics.covariance) {
+      62           0 :     ph_pose_covariance_  = mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped>(nh, Support::toSnakeCase(getName()) + "/pose_covariance", 10);
+      63           0 :     ph_twist_covariance_ = mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped>(nh, Support::toSnakeCase(getName()) + "/twist_covariance", 10);
+      64             :   }
+      65           1 :   if (ch_->debug_topics.innovation) {
+      66           0 :     ph_innovation_ = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh, Support::toSnakeCase(getName()) + "/innovation", 10);
+      67             :   }
+      68           1 :   if (ch_->debug_topics.diag) {
+      69           0 :     ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics>(nh, Support::toSnakeCase(getName()) + "/diagnostics", 10);
+      70             :   }
+      71             : 
+      72             :   // | ------------------ initialize published messages ------------------ |
+      73           1 :   uav_state_init_.header.frame_id = ns_frame_id_;
+      74           1 :   uav_state_init_.child_frame_id  = ch_->frames.ns_fcu;
+      75             : 
+      76           1 :   uav_state_init_.estimator_horizontal = est_lat_name_;
+      77           1 :   uav_state_init_.estimator_vertical   = est_alt_name_;
+      78           1 :   uav_state_init_.estimator_heading    = est_hdg_name_;
+      79             : 
+      80           1 :   innovation_init_.header.frame_id         = ns_frame_id_;
+      81           1 :   innovation_init_.child_frame_id          = ch_->frames.ns_fcu;
+      82           1 :   innovation_init_.pose.pose.orientation.w = 1.0;
+      83             : 
+      84             :   // | ------------------ finish initialization ----------------- |
+      85             : 
+      86           1 :   if (changeState(INITIALIZED_STATE)) {
+      87           1 :     ROS_INFO("[%s]: Estimator initialized", getPrintName().c_str());
+      88             :   } else {
+      89           0 :     ROS_INFO("[%s]: Estimator could not be initialized", getPrintName().c_str());
+      90             :   }
+      91           1 : }
+      92             : /*//}*/
+      93             : 
+      94             : /*//{ start() */
+      95           1 : bool Passthrough::start(void) {
+      96             : 
+      97             : 
+      98           1 :   if (isInState(READY_STATE)) {
+      99             : 
+     100             :     /* timer_update_.start(); */
+     101           1 :     changeState(STARTED_STATE);
+     102           1 :     return true;
+     103             : 
+     104             :   } else {
+     105           0 :     ROS_WARN("[%s]: Estimator must be in READY_STATE to start it", getPrintName().c_str());
+     106           0 :     ros::Duration(1.0).sleep();
+     107             :   }
+     108           0 :   return false;
+     109             : 
+     110             :   ROS_ERROR("[%s]: Failed to start", getPrintName().c_str());
+     111             :   return false;
+     112             : }
+     113             : /*//}*/
+     114             : 
+     115             : /*//{ pause() */
+     116           0 : bool Passthrough::pause(void) {
+     117             : 
+     118           0 :   if (isInState(RUNNING_STATE)) {
+     119           0 :     changeState(STOPPED_STATE);
+     120           0 :     return true;
+     121             :   }
+     122           0 :   return false;
+     123             : }
+     124             : /*//}*/
+     125             : 
+     126             : /*//{ reset() */
+     127           0 : bool Passthrough::reset(void) {
+     128             : 
+     129           0 :   if (!isInitialized()) {
+     130           0 :     ROS_ERROR("[%s]: Cannot reset uninitialized estimator", getPrintName().c_str());
+     131           0 :     return false;
+     132             :   }
+     133             : 
+     134           0 :   changeState(STOPPED_STATE);
+     135             : 
+     136           0 :   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
+     137             : 
+     138           0 :   return true;
+     139             : }
+     140             : /*//}*/
+     141             : 
+     142             : /* timerCheckPassthroughOdomHz() //{*/
+     143           3 : void Passthrough::timerCheckPassthroughOdomHz([[maybe_unused]] const ros::TimerEvent &event) {
+     144             : 
+     145           3 :   if (!isInitialized()) {
+     146           0 :     return;
+     147             :   }
+     148             : 
+     149           3 :   if (isInState(INITIALIZED_STATE)) {
+     150             : 
+     151             :     // first wait for a message
+     152           3 :     if (!sh_passthrough_odom_.hasMsg()) {
+     153           0 :       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());
+     154           0 :       return;
+     155             :     }
+     156             : 
+     157             :     // calculate average rate of messages
+     158           3 :     if (counter_odom_msgs_ > 0) {
+     159           3 :       ros::WallTime t_now  = ros::WallTime::now();
+     160           3 :       double        dt     = (t_now - t_check_hz_last_).toSec();
+     161           3 :       double        avg_hz = counter_odom_msgs_ / dt;
+     162           3 :       t_check_hz_last_     = t_now;
+     163           3 :       counter_odom_msgs_   = 0;
+     164           3 :       ROS_INFO("[%s]: rate of passthrough odom: %.2f Hz", getPrintName().c_str(), avg_hz);
+     165             : 
+     166             :       // check message rate stability
+     167           3 :       if (abs(avg_hz - prev_avg_hz_) >= 5) {
+     168           2 :         ROS_INFO("[%s]: %s stable passthrough odometry rate. now: %.2f Hz prev: %.2f Hz", getPrintName().c_str(), Support::waiting_for_string.c_str(), avg_hz,
+     169             :                  prev_avg_hz_);
+     170           2 :         prev_avg_hz_ = avg_hz;
+     171           2 :         return;
+     172             :       }
+     173             : 
+     174             :       // the message rate must be higher than required by the control manager
+     175           1 :       if (!kickoff_ && avg_hz < ch_->desired_uav_state_rate) {
+     176           0 :         ROS_ERROR(
+     177             :             "[%s]: rate of passthrough odom: %.2f Hz is lower than desired uav_state rate: %.2f Hz. Flight not allowed. Provide higher passthrough odometry rate "
+     178             :             "or use a higher-level controller.",
+     179             :             getPrintName().c_str(), avg_hz, ch_->desired_uav_state_rate);
+     180             :         // note: might run the publishing asynchronously on the desired rate in this case to still be able to fly
+     181             :         // the states would then be interpolated by ZOH (bleeh)
+     182             :         /* timer_update_       = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &Passthrough::timerUpdate, this); */
+     183           0 :         return;
+     184             :       }
+     185             :     }
+     186             : 
+     187           1 :     changeState(READY_STATE);
+     188           1 :     ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is ready to start", getPrintName().c_str());
+     189           1 :     timer_check_passthrough_odom_hz_.stop();
+     190             :   }
+     191             : }
+     192             : /*//}*/
+     193             : 
+     194             : /* timerUpdate() //{*/
+     195           0 : void Passthrough::timerUpdate([[maybe_unused]] const ros::TimerEvent &event) {
+     196             : 
+     197             : 
+     198           0 :   if (isInState(STARTED_STATE)) {
+     199             : 
+     200           0 :     changeState(RUNNING_STATE);
+     201             :   }
+     202             : 
+     203           0 :   const ros::Time time_now = ros::Time::now();
+     204             : 
+     205           0 :   nav_msgs::OdometryConstPtr msg = sh_passthrough_odom_.getMsg();
+     206             : 
+     207           0 :   if (first_iter_) {
+     208           0 :     prev_msg_   = msg;
+     209           0 :     first_iter_ = false;
+     210             :   }
+     211             : 
+     212           0 :   mrs_msgs::UavState uav_state = uav_state_init_;
+     213             : 
+     214           0 :   uav_state.header.stamp = time_now;
+     215             : 
+     216           0 :   uav_state.pose.position    = msg->pose.pose.position;
+     217           0 :   uav_state.pose.orientation = msg->pose.pose.orientation;
+     218             : 
+     219           0 :   uav_state.velocity.linear  = Support::rotateVector(msg->twist.twist.linear, msg->pose.pose.orientation);
+     220           0 :   uav_state.velocity.angular = msg->twist.twist.angular;
+     221             : 
+     222           0 :   const nav_msgs::Odometry odom = Support::uavStateToOdom(uav_state);
+     223             : 
+     224           0 :   nav_msgs::Odometry innovation = innovation_init_;
+     225           0 :   innovation.header.stamp       = time_now;
+     226             : 
+     227           0 :   innovation.pose.pose.position.x = prev_msg_->pose.pose.position.x - msg->pose.pose.position.x;
+     228           0 :   innovation.pose.pose.position.y = prev_msg_->pose.pose.position.y - msg->pose.pose.position.y;
+     229           0 :   innovation.pose.pose.position.z = prev_msg_->pose.pose.position.z - msg->pose.pose.position.z;
+     230             : 
+     231           0 :   mrs_msgs::Float64ArrayStamped pose_covariance, twist_covariance;
+     232           0 :   pose_covariance.header.stamp  = time_now;
+     233           0 :   twist_covariance.header.stamp = time_now;
+     234             : 
+     235           0 :   const int n_states = 6;  // TODO this should be defined somewhere else
+     236           0 :   pose_covariance.values.resize(n_states * n_states);
+     237           0 :   pose_covariance.values.at(n_states * AXIS_X + AXIS_X) = 1e-10;
+     238           0 :   pose_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = 1e-10;
+     239           0 :   pose_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = 1e-10;
+     240             : 
+     241           0 :   twist_covariance.values.resize(n_states * n_states);
+     242           0 :   twist_covariance.values.at(n_states * AXIS_X + AXIS_X) = 1e-10;
+     243           0 :   twist_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = 1e-10;
+     244           0 :   twist_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = 1e-10;
+     245             : 
+     246           0 :   mrs_lib::set_mutexed(mtx_uav_state_, uav_state, uav_state_);
+     247           0 :   mrs_lib::set_mutexed(mtx_odom_, odom, odom_);
+     248           0 :   mrs_lib::set_mutexed(mtx_innovation_, innovation, innovation_);
+     249           0 :   mrs_lib::set_mutexed(mtx_covariance_, pose_covariance, pose_covariance_);
+     250           0 :   mrs_lib::set_mutexed(mtx_covariance_, twist_covariance, twist_covariance_);
+     251             : 
+     252           0 :   publishUavState();
+     253           0 :   publishOdom();
+     254           0 :   publishCovariance();
+     255           0 :   publishInnovation();
+     256           0 :   publishDiagnostics();
+     257             : 
+     258           0 :   prev_msg_ = msg;
+     259           0 : }
+     260             : /*//}*/
+     261             : 
+     262             : /*//{ callbackPassthroughOdom() */
+     263        1317 : void Passthrough::callbackPassthroughOdom(const nav_msgs::Odometry::ConstPtr msg) {
+     264             : 
+     265        1317 :   counter_odom_msgs_++;
+     266             : 
+     267        1317 :   if (!isInitialized()) {
+     268           0 :     return;
+     269             :   }
+     270             : 
+     271        1317 :   if (isInState(STARTED_STATE)) {
+     272             : 
+     273           1 :     changeState(RUNNING_STATE);
+     274             :   }
+     275             : 
+     276        1317 :   if (first_iter_) {
+     277           1 :     prev_msg_   = msg;
+     278           1 :     first_iter_ = false;
+     279             :   }
+     280             : 
+     281        2634 :   nav_msgs::Odometry odom = *msg;
+     282             : 
+     283             :   // change the frame_ids
+     284        1317 :   odom.header.frame_id = ns_frame_id_;
+     285        1317 :   odom.child_frame_id  = ch_->frames.ns_fcu;
+     286             : 
+     287        2634 :   mrs_msgs::UavState uav_state = uav_state_init_;
+     288             : 
+     289             :   // here we are assuming the passthrough odometry has correct timestamp
+     290        1317 :   uav_state.header.stamp = odom.header.stamp;
+     291             :   /* const ros::Time time_now = ros::Time::now(); */
+     292             :   /* uav_state.header.stamp = time_now; */
+     293             : 
+     294        1317 :   uav_state.pose.position    = odom.pose.pose.position;
+     295        1317 :   uav_state.pose.orientation = odom.pose.pose.orientation;
+     296        1317 :   uav_state.velocity.angular = odom.twist.twist.angular;
+     297             : 
+     298             :   // uav_state has velocities in the parent frame
+     299        1317 :   uav_state.velocity.linear = Support::rotateVector(odom.twist.twist.linear, odom.pose.pose.orientation);
+     300             : 
+     301             : 
+     302        2634 :   nav_msgs::Odometry innovation = innovation_init_;
+     303        1317 :   innovation.header.stamp       = odom.header.stamp;
+     304             : 
+     305        1317 :   innovation.pose.pose.position.x = prev_msg_->pose.pose.position.x - msg->pose.pose.position.x;
+     306        1317 :   innovation.pose.pose.position.y = prev_msg_->pose.pose.position.y - msg->pose.pose.position.y;
+     307        1317 :   innovation.pose.pose.position.z = prev_msg_->pose.pose.position.z - msg->pose.pose.position.z;
+     308             : 
+     309        2634 :   mrs_msgs::Float64ArrayStamped pose_covariance, twist_covariance;
+     310        1317 :   pose_covariance.header.stamp  = odom.header.stamp;
+     311        1317 :   twist_covariance.header.stamp = odom.header.stamp;
+     312             : 
+     313        1317 :   const int n_states = 6;  // TODO this should be defined somewhere else
+     314        1317 :   pose_covariance.values.resize(n_states * n_states);
+     315        1317 :   pose_covariance.values.at(n_states * AXIS_X + AXIS_X) = 1e-10;
+     316        1317 :   pose_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = 1e-10;
+     317        1317 :   pose_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = 1e-10;
+     318             : 
+     319        1317 :   twist_covariance.values.resize(n_states * n_states);
+     320        1317 :   twist_covariance.values.at(n_states * AXIS_X + AXIS_X) = 1e-10;
+     321        1317 :   twist_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = 1e-10;
+     322        1317 :   twist_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = 1e-10;
+     323             : 
+     324        1317 :   mrs_lib::set_mutexed(mtx_uav_state_, uav_state, uav_state_);
+     325        1317 :   mrs_lib::set_mutexed(mtx_odom_, odom, odom_);
+     326        1317 :   mrs_lib::set_mutexed(mtx_innovation_, innovation, innovation_);
+     327        1317 :   mrs_lib::set_mutexed(mtx_covariance_, pose_covariance, pose_covariance_);
+     328        1317 :   mrs_lib::set_mutexed(mtx_covariance_, twist_covariance, twist_covariance_);
+     329             : 
+     330        1317 :   publishUavState();
+     331        1317 :   publishOdom();
+     332        1317 :   publishCovariance();
+     333        1317 :   publishInnovation();
+     334        1317 :   publishDiagnostics();
+     335             : 
+     336        1317 :   prev_msg_ = msg;
+     337             : }
+     338             : /*//}*/
+     339             : 
+     340             : /*//{ isConverged() */
+     341           0 : bool Passthrough::isConverged() {
+     342             : 
+     343             :   // TODO: check convergence by rate of change of determinant
+     344             :   // most likely not used in top-level estimator
+     345             : 
+     346           0 :   return true;
+     347             : }
+     348             : /*//}*/
+     349             : 
+     350             : /*//{ setUavState() */
+     351           0 : bool Passthrough::setUavState([[maybe_unused]] const mrs_msgs::UavState &uav_state) {
+     352             : 
+     353           0 :   if (!isInState(STOPPED_STATE)) {
+     354           0 :     ROS_WARN("[%s]: Estimator state can be set only in the STOPPED state", getPrintName().c_str());
+     355           0 :     return false;
+     356             :   }
+     357             : 
+     358           0 :   ROS_WARN("[%s]: Setting the state of this estimator is not implemented.", getPrintName().c_str());
+     359           0 :   return false;
+     360             : }
+     361             : /*//}*/
+     362             : 
+     363             : }  // namespace passthrough
+     364             : 
+     365             : }  // namespace mrs_uav_state_estimators
+     366             : 
+     367             : #include <pluginlib/class_list_macros.h>
+     368           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..6ad5851c84 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.overview.html @@ -0,0 +1,112 @@ + + + + + + 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 + overview + overview + overview + 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..7fc60227d610dd5ac38ca75a9158550b7d12a0b5 GIT binary patch literal 1608 zcmV-O2DkZ%P)=YyeOhGch{8RY~6ci-|1?riZ&zsk;m)o_oU6|=N|9-#a{F&Zr zr#vT=ZT%9L*Nj6pE)ttw*Njv*WPhOAr)6oX-Zr(}n}Wyq;j{|dv~F5&`cY;gNe?ZZSpO zg^M!d(y`#Q;LI?pK`V+Q6B2?e12sJ88xZ-Jp3)Tddp&1$*z{UarP#)3G~dOO!~hI_ zSAgzwou0Em9V9Zk@2ZE%PX3uVXVTJd#Ve$hOc4+3O1&9lqyE3 zP(8$@5V0X=vYuKlTt0$mip%Fhb})^@p1!9!^F}`Gt#lgJoreQGkP=C*77cQsg^g=xk5qC+4^%X45Uy+_ca9pqESWXKw&G*Gte<~1Gox- zngb5oc)bfUYe0>G2wR?pPv9$yxcWfav7Dg;RJqF2k}Iwc(jV;ibX7}rJqQw0p9v54 z{4uDXcJ~?KsJBkEO89__JE2R0V2`^=hZLbNT}#*ZRpWhl#e1?~uc_W#gqo`5HZFen zYqncG8Yo9&{p-RMcOlt&6c_I9OTG!8Wb&@*Bkd>7yv)CxFu}t_z%;`n9$J19X(wI^xc_fc$cx*v6q+y}rQzT}>$;f2Ry;pg?Fp-_ zD?ItAcVsKrQHg>A|FiW-_~V%)Xv;)pTF$}eJqyh!k)MNCSRe^v^g)@4_gwO6c=EjC zaxPM<>0E_N(yJa(p}H<>$W#s8RTjt-C<|nNq4^2ePsBynGq06m3*!oSTzhMV9znqC z>E^d5gf%DuF<)^b$&>00B(`ka-2Sb)&;)I#Y90N$Iu*9IYcU-7ZD3NFN?T*ZBc-WJ z`<7|NMk?6z=-By#V zd=Q8R66~7Ev=bYIN3;bHjZ)M5=X?1sj<2{Js3K>eLMM+!!W`t3BuW|0KeBm~?s z&2`|1yAP9voGKa%xEIF(MHc{(!=oW+3quiW()fEUb} znvF+a06V=&*DC~ zJO^Bh$VPpgC5xPKyh9!M;Y~p0^gqwck7oO-u?L4|ohO zguZlTo4x;-o76|o_Qp}=%1OFO!0`=83Y6N&-IJnkd;uJ^G0Kb8!2m1aVNHAxM;&-_ zrB&qg69X6q_u@UN(slO&FrI5<6Tlb14zF)51hWNnFy}ZvT=@d{z?s1H=MLQ~{C0($ zmxvBjs?HRuTsMID;^K(cLSRoE-v_?77~Xurw1DDUb{{b8u={Au(#PvGcRCje=RDy| z?EW9E!a3O-4}8%QW~luW_bCQ5g9wGhDO?Z3SF~87ox5;q!ecsE{0r{)c#6*YOAWzZ z1CH9jX#VY&zLt4OC#bK;&LqOo>7y3=XG`LJnklq%X8I2kM~YnB0y8rJ0000 + + + + + + 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-12-17 22:35:29Functions: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..68666388ff --- /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-12-17 22:35:29Functions: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..aa37b21db6 --- /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-12-17 22:35:29Functions: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..b6be6f3fea --- /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-12-17 22:35:29Functions: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..402bf27a67 --- /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-12-17 22:35:29Functions: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..78f6e12e46 --- /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-12-17 22:35:29Functions: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..f3d4b591e3 --- /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:19529665.9 %
Date:2024-12-17 22:35:29Functions: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&)116
mrs_uav_state_estimators::StateGeneric::start()8316
mrs_uav_state_estimators::StateGeneric::updateUavState()210234
mrs_uav_state_estimators::StateGeneric::timerPubAttitude(ros::TimerEvent const&)222658
mrs_uav_state_estimators::StateGeneric::timerUpdate(ros::TimerEvent const&)222659
mrs_uav_state_estimators::StateGeneric::getHeading() const336316
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()() const336325
+
+
+ + + +
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..c1d5142b73 --- /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:19529665.9 %
Date:2024-12-17 22:35:29Functions: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&)116
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&)222659
mrs_uav_state_estimators::StateGeneric::updateUavState()210234
mrs_uav_state_estimators::StateGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::StateGeneric::timerPubAttitude(ros::TimerEvent const&)222658
mrs_uav_state_estimators::StateGeneric::pause()0
mrs_uav_state_estimators::StateGeneric::reset()0
mrs_uav_state_estimators::StateGeneric::start()8316
mrs_uav_state_estimators::StateGeneric::getHeading() const336316
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()() const336325
+
+
+ + + +
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..bc1b3ec3bf --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.html @@ -0,0 +1,638 @@ + + + + + + + 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:19529665.9 %
Date:2024-12-17 22:35:29Functions: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         116 : void StateGeneric::initialize(ros::NodeHandle &parent_nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      12             : 
+      13         116 :   ch_ = ch;
+      14         116 :   ph_ = ph;
+      15             : 
+      16         232 :   ros::NodeHandle nh(parent_nh);
+      17             : 
+      18         116 :   if (is_core_plugin_) {
+      19             : 
+      20         116 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml");
+      21         116 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml");
+      22             :   }
+      23             : 
+      24         116 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getName() + "/");
+      25             : 
+      26         116 :   ph->param_loader->loadParam("estimators/lateral/name", est_lat_name_);
+      27         116 :   ph->param_loader->loadParam("estimators/altitude/name", est_alt_name_);
+      28         116 :   ph->param_loader->loadParam("estimators/heading/name", est_hdg_name_);
+      29         116 :   ph->param_loader->loadParam("estimators/heading/passthrough", is_hdg_passthrough_);
+      30             : 
+      31         116 :   ph->param_loader->loadParam("override_frame_id/enabled", is_override_frame_id_);
+      32         116 :   if (is_override_frame_id_) {
+      33           0 :     ph->param_loader->loadParam("override_frame_id/frame_id", frame_id_);
+      34             :   }
+      35             : 
+      36         232 :   std::string topic_orientation;
+      37         116 :   ph->param_loader->loadParam("topics/orientation", topic_orientation);
+      38         116 :   topic_orientation_ = "/" + ch_->uav_name + "/" + topic_orientation;
+      39         232 :   std::string topic_angular_velocity;
+      40         116 :   ph->param_loader->loadParam("topics/angular_velocity", topic_angular_velocity);
+      41         116 :   topic_angular_velocity_ = "/" + ch_->uav_name + "/" + topic_angular_velocity;
+      42             : 
+      43         116 :   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         116 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      49             : 
+      50             :   // | ------------------ timers initialization ----------------- |
+      51         116 :   timer_update_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &StateGeneric::timerUpdate, this);
+      52             :   /* timer_check_health_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &StateGeneric::timerCheckHealth, this); */
+      53         116 :   timer_pub_attitude_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &StateGeneric::timerPubAttitude, this);
+      54             : 
+      55             :   // | --------------- subscribers initialization --------------- |
+      56         232 :   mrs_lib::SubscribeHandlerOptions shopts;
+      57         116 :   shopts.nh                 = nh;
+      58         116 :   shopts.node_name          = getPrintName();
+      59         116 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      60         116 :   shopts.threadsafe         = true;
+      61         116 :   shopts.autostart          = true;
+      62         116 :   shopts.queue_size         = 10;
+      63         116 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      64             : 
+      65         116 :   sh_hw_api_orient_  = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, topic_orientation_);
+      66         116 :   sh_hw_api_ang_vel_ = mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped>(shopts, topic_angular_velocity_);
+      67             : 
+      68             :   // | ---------------- publishers initialization --------------- |
+      69         116 :   ph_odom_     = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh, Support::toSnakeCase(getName()) + "/odom", 10);
+      70         116 :   ph_attitude_ = mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped>(nh, Support::toSnakeCase(getName()) + "/attitude", 10);
+      71             : 
+      72         116 :   if (ch_->debug_topics.state) {
+      73         116 :     ph_uav_state_ = mrs_lib::PublisherHandler<mrs_msgs::UavState>(nh, Support::toSnakeCase(getName()) + "/uav_state", 10);
+      74             :   }
+      75         116 :   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         116 :   if (ch_->debug_topics.innovation) {
+      80           0 :     ph_innovation_ = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh, Support::toSnakeCase(getName()) + "/innovation", 10);
+      81             :   }
+      82         116 :   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         232 :   std::vector<double> max_altitudes;
+      88             : 
+      89         116 :   if (is_hdg_passthrough_) {
+      90         116 :     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         116 :   est_hdg_->initialize(nh, ch_, ph_);
+      95         116 :   max_altitudes.push_back(est_hdg_->getMaxFlightZ());
+      96             : 
+      97      336441 :   est_lat_ = std::make_unique<LatGeneric>(est_lat_name_, frame_id_, getName(), is_core_plugin_, [this](void) { return this->getHeading(); });
+      98         116 :   est_lat_->initialize(nh, ch_, ph_);
+      99         116 :   max_altitudes.push_back(est_lat_->getMaxFlightZ());
+     100             : 
+     101         116 :   est_alt_ = std::make_unique<AltGeneric>(est_alt_name_, frame_id_, getName(), is_core_plugin_);
+     102         116 :   est_alt_->initialize(nh, ch_, ph_);
+     103         116 :   max_altitudes.push_back(est_alt_->getMaxFlightZ());
+     104             : 
+     105         116 :   max_flight_z_ = *std::min_element(max_altitudes.begin(), max_altitudes.end());
+     106             : 
+     107             :   // | ------------------ initialize published messages ------------------ |
+     108         116 :   uav_state_init_.header.frame_id = ns_frame_id_;
+     109         116 :   uav_state_init_.child_frame_id  = ch_->frames.ns_fcu;
+     110             : 
+     111         116 :   uav_state_init_.estimator_horizontal = est_lat_name_;
+     112         116 :   uav_state_init_.estimator_vertical   = est_alt_name_;
+     113         116 :   uav_state_init_.estimator_heading    = est_hdg_name_;
+     114             : 
+     115         116 :   innovation_init_.header.frame_id         = ns_frame_id_;
+     116         116 :   innovation_init_.child_frame_id          = ch_->frames.ns_fcu;
+     117         116 :   innovation_init_.pose.pose.orientation.w = 1.0;
+     118             : 
+     119             :   // | ------------------ finish initialization ----------------- |
+     120             : 
+     121         116 :   if (changeState(INITIALIZED_STATE)) {
+     122         116 :     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         116 : }
+     127             : /*//}*/
+     128             : 
+     129             : /*//{ start() */
+     130        8316 : bool StateGeneric::start(void) {
+     131             : 
+     132        8316 :   if (isInState(READY_STATE)) {
+     133             : 
+     134             :     bool est_lat_start_successful, est_alt_start_successful, est_hdg_start_successful;
+     135             : 
+     136        8316 :     if (est_lat_->isStarted() || est_lat_->isRunning()) {
+     137        3986 :       est_lat_start_successful = true;
+     138             :     } else {
+     139        4330 :       est_lat_start_successful = est_lat_->start();
+     140             :     }
+     141             : 
+     142        8316 :     if (est_alt_->isStarted() || est_alt_->isRunning()) {
+     143        3903 :       est_alt_start_successful = true;
+     144             :     } else {
+     145        4413 :       est_alt_start_successful = est_alt_->start();
+     146             :     }
+     147             : 
+     148        8316 :     if (est_hdg_->isStarted() || est_hdg_->isRunning()) {
+     149        8130 :       timer_pub_attitude_.start();
+     150        8130 :       est_hdg_start_successful = true;
+     151             :     } else {
+     152         186 :       est_hdg_start_successful = est_hdg_->start();
+     153             :     }
+     154             : 
+     155        8316 :     if (est_lat_start_successful && est_alt_start_successful && est_hdg_start_successful) {
+     156             :       /* timer_update_.start(); */
+     157         116 :       changeState(STARTED_STATE);
+     158         116 :       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        8200 :   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 :   changeState(STOPPED_STATE);
+     197           0 :   est_lat_->reset();
+     198           0 :   est_alt_->reset();
+     199           0 :   est_hdg_->reset();
+     200           0 :   changeState(INITIALIZED_STATE);
+     201             : 
+     202           0 :   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
+     203             : 
+     204           0 :   return true;
+     205             : }
+     206             : /*//}*/
+     207             : 
+     208             : /* timerUpdate() //{*/
+     209      222659 : void StateGeneric::timerUpdate([[maybe_unused]] const ros::TimerEvent &event) {
+     210             : 
+     211             : 
+     212      222659 :   if (!isInitialized()) {
+     213       12420 :     return;
+     214             :   }
+     215             : 
+     216      443642 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::timerUpdate", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     217             : 
+     218      221821 :   switch (getCurrentSmState()) {
+     219             : 
+     220           0 :     case UNINITIALIZED_STATE: {
+     221           0 :       break;
+     222             :     }
+     223        3265 :     case INITIALIZED_STATE: {
+     224             : 
+     225        3265 :       if (sh_hw_api_orient_.hasMsg() && sh_hw_api_ang_vel_.hasMsg()) {
+     226         116 :         if (est_lat_->isInitialized() && est_alt_->isInitialized() && est_hdg_->isInitialized()) {
+     227         116 :           changeState(READY_STATE);
+     228         116 :           ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is ready to start", getPrintName().c_str());
+     229             :         } else {
+     230           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s subestimators to be initialized", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     231           0 :           return;
+     232             :         }
+     233             :       } else {
+     234        3149 :         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());
+     235        3149 :         return;
+     236             :       }
+     237             : 
+     238         116 :       break;
+     239             :     }
+     240             : 
+     241        8282 :     case READY_STATE: {
+     242        8282 :       break;
+     243             :     }
+     244             : 
+     245         152 :     case STARTED_STATE: {
+     246             : 
+     247         152 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     248             : 
+     249         152 :       if (est_lat_->isError() || est_alt_->isError() || est_hdg_->isError()) {
+     250           0 :         changeState(ERROR_STATE);
+     251             :       }
+     252             : 
+     253         152 :       if (est_lat_->isRunning() && est_alt_->isRunning() && est_hdg_->isRunning()) {
+     254         116 :         ROS_INFO_THROTTLE(1.0, "[%s]: Subestimators converged", getPrintName().c_str());
+     255         116 :         changeState(RUNNING_STATE);
+     256             :       } else {
+     257          36 :         return;
+     258             :       }
+     259         116 :       break;
+     260             :     }
+     261             : 
+     262      210123 :     case RUNNING_STATE: {
+     263      210123 :       if (est_lat_->isError() || est_alt_->isError() || est_hdg_->isError()) {
+     264           0 :         changeState(ERROR_STATE);
+     265             :       }
+     266      210120 :       break;
+     267             :     }
+     268             : 
+     269           0 :     case STOPPED_STATE: {
+     270           0 :       break;
+     271             :     }
+     272             : 
+     273           0 :     case ERROR_STATE: {
+     274           0 :       if ((est_lat_->isReady() || est_lat_->isRunning()) && (est_alt_->isReady() || est_alt_->isRunning()) && (est_hdg_->isReady() || est_hdg_->isRunning())) {
+     275           0 :         changeState(READY_STATE);
+     276             :       }
+     277           0 :       break;
+     278             :     }
+     279             :   }
+     280             : 
+     281      218631 :   if (!isRunning() && !isStarted()) {
+     282        8398 :     return;
+     283             :   }
+     284             : 
+     285      210231 :   updateUavState();
+     286             : 
+     287      210234 :   publishUavState();
+     288      210239 :   publishOdom();
+     289      210239 :   publishCovariance();
+     290      210239 :   publishInnovation();
+     291      210239 :   publishDiagnostics();
+     292             : }
+     293             : /*//}*/
+     294             : 
+     295             : /*//{ timerCheckHealth() */
+     296           0 : void StateGeneric::timerCheckHealth([[maybe_unused]] const ros::TimerEvent &event) {
+     297             : 
+     298           0 :   if (!isInitialized()) {
+     299           0 :     return;
+     300             :   }
+     301             : 
+     302           0 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::timerCheckHealth", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     303             : 
+     304           0 :   switch (getCurrentSmState()) {
+     305             : 
+     306           0 :     case UNINITIALIZED_STATE: {
+     307           0 :       break;
+     308             :     }
+     309           0 :     case INITIALIZED_STATE: {
+     310             : 
+     311           0 :       if (sh_hw_api_orient_.hasMsg() && sh_hw_api_ang_vel_.hasMsg()) {
+     312           0 :         if (est_lat_->isInitialized() && est_alt_->isInitialized() && est_hdg_->isInitialized()) {
+     313           0 :           changeState(READY_STATE);
+     314           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is ready to start", getPrintName().c_str());
+     315             :         } else {
+     316           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s subestimators to be initialized", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     317           0 :           return;
+     318             :         }
+     319             :       } else {
+     320           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());
+     321           0 :         return;
+     322             :       }
+     323             : 
+     324           0 :       break;
+     325             :     }
+     326             : 
+     327           0 :     case READY_STATE: {
+     328           0 :       break;
+     329             :     }
+     330             : 
+     331           0 :     case STARTED_STATE: {
+     332             : 
+     333           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s for convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     334             : 
+     335           0 :       if (est_lat_->isError() || est_alt_->isError() || est_hdg_->isError()) {
+     336           0 :         changeState(ERROR_STATE);
+     337             :       }
+     338             : 
+     339           0 :       if (est_lat_->isRunning() && est_alt_->isRunning() && est_hdg_->isRunning()) {
+     340           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: Subestimators converged", getPrintName().c_str());
+     341           0 :         changeState(RUNNING_STATE);
+     342             :       } else {
+     343           0 :         return;
+     344             :       }
+     345           0 :       break;
+     346             :     }
+     347             : 
+     348           0 :     case RUNNING_STATE: {
+     349           0 :       if (est_lat_->isError() || est_alt_->isError() || est_hdg_->isError()) {
+     350           0 :         changeState(ERROR_STATE);
+     351             :       }
+     352           0 :       break;
+     353             :     }
+     354             : 
+     355           0 :     case STOPPED_STATE: {
+     356           0 :       break;
+     357             :     }
+     358             : 
+     359           0 :     case ERROR_STATE: {
+     360           0 :       if (est_lat_->isReady() && est_alt_->isReady() && est_hdg_->isReady()) {
+     361           0 :         changeState(READY_STATE);
+     362             :       }
+     363           0 :       break;
+     364             :     }
+     365             :   }
+     366             : }
+     367             : /*//}*/
+     368             : 
+     369             : /* timerPubAttitude() //{*/
+     370      222658 : void StateGeneric::timerPubAttitude([[maybe_unused]] const ros::TimerEvent &event) {
+     371             : 
+     372      222658 :   if (!isInitialized()) {
+     373        4334 :     return;
+     374             :   }
+     375             : 
+     376      443639 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::timerPubAttitude", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     377             : 
+     378      221818 :   if (!sh_hw_api_orient_.hasMsg()) {
+     379        1931 :     ROS_WARN_THROTTLE(1.0, "[%s]: has not received orientation on topic %s yet", getPrintName().c_str(), sh_hw_api_orient_.topicName().c_str());
+     380        1931 :     return;
+     381             :   }
+     382             : 
+     383      219886 :   if (!est_hdg_->isRunning() && !isError()) {
+     384        1568 :     ROS_WARN_THROTTLE(1.0, "[%s]: cannot publish attitude, heading estimator is not running", getPrintName().c_str());
+     385        1568 :     return;
+     386             :   }
+     387             : 
+     388      218323 :   scope_timer.checkpoint("checks");
+     389             : 
+     390      218317 :   const ros::Time time_now = ros::Time::now();
+     391             : 
+     392      218324 :   geometry_msgs::QuaternionStamped att;
+     393      218322 :   att.header.stamp    = time_now;
+     394      218322 :   att.header.frame_id = ns_frame_id_ + "_att_only";
+     395             : 
+     396             :   double hdg;
+     397      218322 :   if (isError()) {
+     398           0 :     hdg = est_hdg_->getLastValidHdg();
+     399             :   } else {
+     400      218321 :     hdg = est_hdg_->getState(POSITION);
+     401             :   }
+     402             : 
+     403      218324 :   auto res = rotateQuaternionByHeading(sh_hw_api_orient_.getMsg()->quaternion, hdg);
+     404      218286 :   if (res) {
+     405      218315 :     att.quaternion = res.value();
+     406             :   } else {
+     407           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: could not rotate orientation by heading", getPrintName().c_str());
+     408           0 :     return;
+     409             :   }
+     410             : 
+     411      218268 :   scope_timer.checkpoint("rotate");
+     412             : 
+     413      218310 :   if (!Support::noNans(att.quaternion)) {
+     414           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaNs in timerPubAttitude quaternion", getPrintName().c_str());
+     415           0 :     return;
+     416             :   }
+     417             : 
+     418      218288 :   scope_timer.checkpoint("nan check");
+     419             : 
+     420      218276 :   ph_attitude_.publish(att);
+     421      218324 :   scope_timer.checkpoint("publish");
+     422             : }
+     423             : /*//}*/
+     424             : 
+     425             : /*//{ isConverged() */
+     426           0 : bool StateGeneric::isConverged() {
+     427             : 
+     428             :   // TODO: check convergence by rate of change of determinant
+     429             :   // most likely not used in top-level estimator
+     430             : 
+     431           0 :   return true;
+     432             : }
+     433             : /*//}*/
+     434             : 
+     435             : /*//{ updateUavState() */
+     436      210234 : void StateGeneric::updateUavState() {
+     437             : 
+     438      210234 :   if (!sh_hw_api_orient_.hasMsg()) {
+     439           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());
+     440           0 :     return;
+     441             :   }
+     442             : 
+     443      210235 :   if (!sh_hw_api_ang_vel_.hasMsg()) {
+     444           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());
+     445           0 :     return;
+     446             :   }
+     447      420475 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::updateUavState", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     448             : 
+     449      210238 :   const ros::Time time_now = ros::Time::now();
+     450             : 
+     451      210239 :   mrs_msgs::UavState uav_state = uav_state_init_;
+     452      210239 :   uav_state.header.stamp       = time_now;
+     453             : 
+     454             :   // do not rotate orientation if passthrough hdg
+     455      210239 :   if (est_hdg_name_ == "hdg_passthrough") {
+     456           0 :     uav_state.pose.orientation = sh_hw_api_orient_.getMsg()->quaternion;
+     457             :   } else {
+     458             :     double hdg;
+     459      210237 :     if (isError()) {
+     460           0 :       hdg = est_hdg_->getLastValidHdg();
+     461             :     } else {
+     462      210238 :       hdg = est_hdg_->getState(POSITION);
+     463             :     }
+     464             : 
+     465      210234 :     auto res = rotateQuaternionByHeading(sh_hw_api_orient_.getMsg()->quaternion, hdg);
+     466      210204 :     if (res) {
+     467      210227 :       uav_state.pose.orientation = res.value();
+     468             :     } else {
+     469           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: could not rotate orientation by heading", getPrintName().c_str());
+     470           0 :       return;
+     471             :     }
+     472             :   }
+     473             : 
+     474      210211 :   scope_timer.checkpoint("rotate orientation");
+     475             : 
+     476      210235 :   uav_state.velocity.angular = sh_hw_api_ang_vel_.getMsg()->vector;
+     477             : 
+     478      210214 :   uav_state.pose.position.x = est_lat_->getState(POSITION, AXIS_X);
+     479      210231 :   uav_state.pose.position.y = est_lat_->getState(POSITION, AXIS_Y);
+     480      210224 :   uav_state.pose.position.z = est_alt_->getState(POSITION);
+     481             : 
+     482      210218 :   uav_state.velocity.linear.x = est_lat_->getState(VELOCITY, AXIS_X);  // in global frame
+     483      210225 :   uav_state.velocity.linear.y = est_lat_->getState(VELOCITY, AXIS_Y);  // in global frame
+     484      210233 :   uav_state.velocity.linear.z = est_alt_->getState(VELOCITY);          // in global frame
+     485             : 
+     486      210230 :   uav_state.acceleration.linear.x = est_lat_->getState(ACCELERATION, AXIS_X);  // in global frame
+     487      210232 :   uav_state.acceleration.linear.y = est_lat_->getState(ACCELERATION, AXIS_Y);  // in global frame
+     488      210233 :   uav_state.acceleration.linear.z = est_alt_->getState(ACCELERATION);          // in global frame
+     489             : 
+     490      210230 :   scope_timer.checkpoint("fill uav state");
+     491             : 
+     492      420447 :   const nav_msgs::Odometry odom = Support::uavStateToOdom(uav_state);
+     493      210236 :   scope_timer.checkpoint("uav state to odom");
+     494             : 
+     495      420452 :   nav_msgs::Odometry innovation = innovation_init_;
+     496      210229 :   innovation.header.stamp       = time_now;
+     497             : 
+     498      210229 :   innovation.pose.pose.position.x = est_lat_->getInnovation(POSITION, AXIS_X);
+     499      210220 :   innovation.pose.pose.position.y = est_lat_->getInnovation(POSITION, AXIS_Y);
+     500      210220 :   innovation.pose.pose.position.z = est_alt_->getInnovation(POSITION);
+     501             : 
+     502      210203 :   is_mitigating_jump_ = est_alt_->isMitigatingJump() || est_lat_->isMitigatingJump() || est_hdg_->isMitigatingJump();
+     503             : 
+     504      210234 :   scope_timer.checkpoint("innovation");
+     505             : 
+     506      420425 :   mrs_msgs::Float64ArrayStamped pose_covariance, twist_covariance;
+     507      210198 :   pose_covariance.header.stamp  = time_now;
+     508      210198 :   twist_covariance.header.stamp = time_now;
+     509             : 
+     510      210198 :   const int n_states = 6;  // TODO this should be defined somewhere else
+     511      210198 :   pose_covariance.values.resize(n_states * n_states);
+     512      210201 :   pose_covariance.values.at(n_states * AXIS_X + AXIS_X) = est_lat_->getCovariance(POSITION, AXIS_X);
+     513      210182 :   pose_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = est_lat_->getCovariance(POSITION, AXIS_Y);
+     514      210217 :   pose_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = est_alt_->getCovariance(POSITION);
+     515             : 
+     516      210189 :   twist_covariance.values.resize(n_states * n_states);
+     517      210213 :   twist_covariance.values.at(n_states * AXIS_X + AXIS_X) = est_lat_->getCovariance(VELOCITY, AXIS_X);
+     518      210223 :   twist_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = est_lat_->getCovariance(VELOCITY, AXIS_Y);
+     519      210226 :   twist_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = est_alt_->getCovariance(VELOCITY);
+     520             : 
+     521      210213 :   scope_timer.checkpoint("covariance");
+     522             : 
+     523      210220 :   mrs_lib::set_mutexed(mtx_uav_state_, uav_state, uav_state_);
+     524      210204 :   mrs_lib::set_mutexed(mtx_odom_, odom, odom_);
+     525      210188 :   mrs_lib::set_mutexed(mtx_innovation_, innovation, innovation_);
+     526      210215 :   mrs_lib::set_mutexed(mtx_covariance_, pose_covariance, pose_covariance_);
+     527      210206 :   mrs_lib::set_mutexed(mtx_covariance_, twist_covariance, twist_covariance_);
+     528             : }
+     529             : /*//}*/
+     530             : 
+     531             : /*//{ getHeading() */
+     532      336316 : std::optional<double> StateGeneric::getHeading() const {
+     533      336316 :   if (!est_hdg_->isRunning()) {
+     534         208 :     return {};
+     535             :   }
+     536      336107 :   return est_hdg_->getState(POSITION);
+     537             : }
+     538             : /*//}*/
+     539             : 
+     540             : /*//{ setUavState() */
+     541           0 : bool StateGeneric::setUavState([[maybe_unused]] const mrs_msgs::UavState &uav_state) {
+     542             : 
+     543           0 :   if (!isInState(STOPPED_STATE)) {
+     544           0 :     ROS_WARN("[%s]: Estimator state can be set only in the STOPPED state", getPrintName().c_str());
+     545           0 :     return false;
+     546             :   }
+     547             : 
+     548           0 :   ROS_WARN("[%s]: Setting the state of this estimator is not implemented.", getPrintName().c_str());
+     549           0 :   return false;
+     550             : }
+     551             : /*//}*/
+     552             : 
+     553             : }  // namespace mrs_uav_state_estimators
+     554             : 
+
+
+
+ + + + +
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..f3994aa7b8 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.overview.html @@ -0,0 +1,159 @@ + + + + + + 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 + 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..e4377eb09d8abf24fdb6942c48ffac3b4297badc GIT binary patch literal 1972 zcmV;l2TS;gP)RX!#_;9zBJ-wboFx9z0)lY|Hk&&Tum9N+(j ztHvAOj#M7a7{|+)BRq}>OP5m~oNnIKE4b%b&FRqos{JR!1dd^vwVw5*fH-N&{yblS zF{npf#N#yAhEi7HC?KH2oiscI%A2_~{h-}4St;D#DxjyWFT*&W^l*Uoj*(3+*@1>- zc@@TLugkIw%XS4utE~^Swo83{S4V(v|5Pe@>ENb>^ejXJ$ktBb1lQrMvE%Af0f130 zn9yuL=54h#ym!~_0|HnvVJ};!Hi^}atLL3)LKYs6lpM0&IC9cFeq>C^6t1$mPi$RA zdPHP(OjmIp%M3R07zHUY(S({po-x{yMr(>BK58L`lN7faBPTnS3(J6L zd~D}YrfWVSEzo78*Hsp|i8MZ%Je+umR4kly+(Lov99Nt&$}=@=;ig!rbA=0r-alQN z-HdT0M$$q#qa#3EkFs`xZuVUGkH;#HQg~#QJ&G|dklgHgu-3pMQ&WW6wI=lx)!(P3 zX0oa*#sqEx0ks5pn#b)^kFx}*DIkH(d(Rv24-~@<1$#VS@+stK7U*!{E34T9w#7#u z2G`fVo-rgmHYX&oq?m@-OJs$^8E!WM(0-ipN(vMPt^i<+n>eiXDZFWLGsdK5Zz#hX zkI`(&mjJ^A`dJPmnYSDxs-AyRcaU1_4E>+g^QNuy?{*%UB0*sa%jO+W(q31ohtU=x z)sbWc{23O*dA_zSY<**Tq$oj*E6q=D~AOWzR6&B1ko9|-9 zLQZJ4i>uQ{rK~-!$BUaTmNI~`%PP)kp%|m4@8Gc-NU;&EG}SS;6y3?y6!zF`rBjv8 zZ)N8US^+>&s?E)$j)sb*&IAgw+bs`Rj6G0Cqr)tme#3F^@}3*jLlS#!^>KA#Kfz;> zMav^uo*7z(8@@ui+JwvX>bZQMZSZgJ(9+I=ZzzTkNU9i!OzW0v zU#Pm1qM>x%^Rju}*>veKT@$Nh6H`t-N2x;2(teM6v<(Fxs)r63PzL^rj%ID&SKQH* z!b`(0yRGJWgp<3Y{k4~9Gv~vd&Um<8pZ0QI>YvzJbo82wt>&~Mm2YW)ZU)z8>B8Qf zp{Lysb9Vq}Kku3fV|=(bYX@PvY=7!jaw&$hc-(FBC%Lj1QPRT}6pr&*K0>53F4$U^ z*HrQ5;_RVPXF{-&g3G-LEcf<>I8yEnU^mR(V@JxpZd(xQ<(^XywnKw`Q!4uKLo5qe z?m58v$ zEcq(|EuW`(RMc!7umPhc_%b<@J+&x&=(EFTq=~)=>7-c@aU=C0uC%pX<6H?ipXM={ z2G>1}OgdJ54rAaeE|p@yf(dhK{M^G`1-Atc`)mcWM_L!2n3j7`R0ePZ(|V3f)dTRP zWr#B88sHAL3r^Eh55NzExz=&FK_132%Ybf6ai#*u@JO!Y=}!{iOy-TQ0MdtFd`0J@ zyt>}j*D32xVpdhQJUC$gI$&%3p-xE`AP@Kr3U6&S9R>NZ8ggw#Y{chXOU)}%{U6WJ zFkwrF`~8OVg?5jXYq5mAS0?=D0J3;%mus|(dFY|L(D)B1DOM|PNwhHl0000~v> literal 0 HcmV?d00001 diff --git a/mrs_uav_trackers/src/joy_tracker/index-detail-sort-f.html b/mrs_uav_trackers/src/joy_tracker/index-detail-sort-f.html new file mode 100644 index 0000000000..33e0edb6fa --- /dev/null +++ b/mrs_uav_trackers/src/joy_tracker/index-detail-sort-f.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-12-17 22:35:29Functions: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..07208bd908 --- /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-12-17 22:35:29Functions: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..638918089b --- /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-12-17 22:35:29Functions: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..c973461ec6 --- /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-12-17 22:35:29Functions: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..07aac07c49 --- /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-12-17 22:35:29Functions: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..6fe03c1fe9 --- /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-12-17 22:35:29Functions: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..3413af1cb4 --- /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-12-17 22:35:29Functions: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()109
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>)109
mrs_uav_trackers::joy_tracker::JoyTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)312
mrs_uav_trackers::joy_tracker::JoyTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)387
mrs_uav_trackers::joy_tracker::JoyTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)142109
+
+
+ + + +
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..49aaad00b0 --- /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-12-17 22:35:29Functions:71838.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()109
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>)109
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&)387
mrs_uav_trackers::joy_tracker::JoyTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)312
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&)142109
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..f06ca033fd --- /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-12-17 22:35:29Functions: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         109 : 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         109 :   this->common_handlers_  = common_handlers;
+     136         109 :   this->private_handlers_ = private_handlers;
+     137             : 
+     138         109 :   _uav_name_ = common_handlers->uav_name;
+     139             : 
+     140         109 :   nh_ = nh;
+     141             : 
+     142         109 :   ros::Time::waitForValid();
+     143             : 
+     144             :   // --------------------------------------------------------------
+     145             :   // |                     loading parameters                     |
+     146             :   // --------------------------------------------------------------
+     147             : 
+     148             :   // | ---------- loading params using the parent's nh ---------- |
+     149             : 
+     150         218 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     151             : 
+     152         109 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     153             : 
+     154         109 :   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         109 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/joy_tracker.yaml");
+     162         109 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/joy_tracker.yaml");
+     163             : 
+     164         218 :   const std::string yaml_prefix = "mrs_uav_trackers/joy_tracker/";
+     165             : 
+     166         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/vertical_speed", _vertical_speed_);
+     167             : 
+     168         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "max_tilt", _max_tilt_);
+     169             : 
+     170         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "heading_tracker/heading_rate", _heading_rate_);
+     171             : 
+     172             :   // load channels
+     173         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/pitch", _channel_pitch_);
+     174         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/roll", _channel_roll_);
+     175         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/heading", _channel_heading_);
+     176         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/throttle", _channel_throttle_);
+     177             : 
+     178             :   // load channel multipliers
+     179         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/pitch", _channel_mult_pitch_);
+     180         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/roll", _channel_mult_roll_);
+     181         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/heading", _channel_mult_heading_);
+     182         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/throttle", _channel_mult_throttle_);
+     183             : 
+     184         109 :   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         109 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "JoyTracker", _profiler_enabled_);
+     192             : 
+     193             :   // | ----------------------- subscribers ---------------------- |
+     194             : 
+     195         109 :   mrs_lib::SubscribeHandlerOptions shopts;
+     196         109 :   shopts.nh              = nh_;
+     197         109 :   shopts.node_name       = "JoyTracker";
+     198         109 :   shopts.queue_size      = 1;
+     199         109 :   shopts.transport_hints = ros::TransportHints().tcpNoDelay();
+     200             : 
+     201         109 :   sh_joystick_ = mrs_lib::SubscribeHandler<sensor_msgs::Joy>(shopts, "joystick");
+     202             : 
+     203             :   // | --------------------- finish the init -------------------- |
+     204             : 
+     205         109 :   last_update = ros::Time(0);
+     206             : 
+     207         109 :   is_initialized_ = true;
+     208             : 
+     209         109 :   ROS_INFO("[JoyTracker]: initialized");
+     210             : 
+     211         109 :   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      142109 : 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      426327 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     300      426327 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("JoyTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     301             : 
+     302             :   {
+     303      142109 :     std::scoped_lock lock(mutex_uav_state_);
+     304             : 
+     305      142109 :     uav_state_ = uav_state;
+     306             : 
+     307      142109 :     got_uav_state_ = true;
+     308             :   }
+     309             : 
+     310      142109 :   double dt = (ros::Time::now() - last_update).toSec();
+     311             : 
+     312      142109 :   last_update = ros::Time::now();
+     313             : 
+     314             :   // up to this part the update() method is evaluated even when the tracker is not active
+     315      142109 :   if (!is_active_) {
+     316      142109 :     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         312 : const std_srvs::SetBoolResponse::ConstPtr JoyTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     388             : 
+     389         624 :   std_srvs::SetBoolResponse res;
+     390         624 :   std::stringstream         ss;
+     391             : 
+     392         312 :   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         292 :     ss << "callbacks were already " << (callbacks_enabled_ ? "enabled" : "disabled");
+     402         292 :     ROS_WARN_STREAM_THROTTLE(1.0, "[JoyTracker]: " << ss.str());
+     403             :   }
+     404             : 
+     405         312 :   res.message = ss.str();
+     406         312 :   res.success = true;
+     407             : 
+     408         624 :   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         387 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr JoyTracker::setConstraints([
+     465             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     466             : 
+     467         387 :   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         109 : 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:44160173.4 %
Date:2024-12-17 22:35:29Functions: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.4%73.4%
+ 		73.4 %441 / 60167.7 %21 / 31
<unnamed>73.4 %441 / 60167.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..2900825830 --- /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:44160173.4 %
Date:2024-12-17 22:35:29Functions: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.4%73.4%
+ 		73.4 %441 / 60167.7 %21 / 31
<unnamed>73.4 %441 / 60167.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..451e6bbdf4 --- /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:44160173.4 %
Date:2024-12-17 22:35:29Functions: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.4%73.4%
+ 		73.4 %441 / 60167.7 %21 / 31
<unnamed>73.4 %441 / 60167.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..224cec5377 --- /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:44160173.4 %
Date:2024-12-17 22:35:29Functions: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.4%73.4%
+ 		73.4 %441 / 60167.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..7156cb3bf3 --- /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:44160173.4 %
Date:2024-12-17 22:35:29Functions: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.4%73.4%
+ 		73.4 %441 / 60167.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..ff4aae8ed3 --- /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:44160173.4 %
Date:2024-12-17 22:35:29Functions: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.4%73.4%
+ 		73.4 %441 / 60167.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..c3cb10f866 --- /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:44160173.4 %
Date:2024-12-17 22:35:29Functions: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> >&)21
mrs_uav_trackers::landoff_tracker::LandoffTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)46
mrs_uav_trackers::landoff_tracker::LandoffTracker::deactivate()62
(anonymous namespace)::ProxyExec0::ProxyExec0()109
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>)109
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeState(mrs_uav_trackers::landoff_tracker::States_t)151
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeStateHorizontal(mrs_uav_trackers::landoff_tracker::States_t)185
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeStateVertical(mrs_uav_trackers::landoff_tracker::States_t)227
mrs_uav_trackers::landoff_tracker::LandoffTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)312
mrs_uav_trackers::landoff_tracker::LandoffTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)387
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopVerticalMotion()682
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopHorizontalMotion()682
mrs_uav_trackers::landoff_tracker::LandoffTracker::getStatus()2288
mrs_uav_trackers::landoff_tracker::LandoffTracker::decelerateVertical()4797
mrs_uav_trackers::landoff_tracker::LandoffTracker::accelerateVertical()16042
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopHorizontal()20839
mrs_uav_trackers::landoff_tracker::LandoffTracker::timerMain(ros::TimerEvent const&)23912
mrs_uav_trackers::landoff_tracker::LandoffTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)142109
+
+
+ + + +
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..6bc95e13e7 --- /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:44160173.4 %
Date:2024-12-17 22:35:29Functions:213167.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()109
mrs_uav_trackers::landoff_tracker::LandoffTracker::deactivate()62
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>)109
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeState(mrs_uav_trackers::landoff_tracker::States_t)151
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&)387
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopHorizontal()20839
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackTakeoff(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)21
mrs_uav_trackers::landoff_tracker::LandoffTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)312
mrs_uav_trackers::landoff_tracker::LandoffTracker::accelerateVertical()16042
mrs_uav_trackers::landoff_tracker::LandoffTracker::decelerateVertical()4797
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopVerticalMotion()682
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeStateVertical(mrs_uav_trackers::landoff_tracker::States_t)227
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()682
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)185
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&)142109
mrs_uav_trackers::landoff_tracker::LandoffTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)46
mrs_uav_trackers::landoff_tracker::LandoffTracker::getStatus()2288
mrs_uav_trackers::landoff_tracker::LandoffTracker::timerMain(ros::TimerEvent const&)23912
+
+
+ + + +
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..593476b907 --- /dev/null +++ b/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.html @@ -0,0 +1,1659 @@ + + + + + + + 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:44160173.4 %
Date:2024-12-17 22:35:29Functions: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 std::array<const char*, 7> state_names = {
+      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             :   std::atomic<bool> activate_as_the_first_tracker = false;
+     107             : 
+     108             :   // | ------------------------ uav state ----------------------- |
+     109             : 
+     110             :   mrs_msgs::UavState uav_state_;
+     111             :   bool               got_uav_state_ = false;
+     112             :   std::mutex         mutex_uav_state_;
+     113             : 
+     114             :   // | ---------------- the tracker's inner state --------------- |
+     115             : 
+     116             :   int    _main_timer_rate_;
+     117             :   double _landing_reference_;
+     118             :   double _tracker_dt_;
+     119             :   bool   is_initialized_ = false;
+     120             :   bool   is_active_      = false;
+     121             : 
+     122             :   bool   _takeoff_disable_lateral_gains_ = false;
+     123             :   double _takeoff_disable_lateral_gains_z_;
+     124             : 
+     125             :   // | --------------- the tracker's state machine -------------- |
+     126             : 
+     127             :   States_t current_state_vertical_    = IDLE_STATE;
+     128             :   States_t previous_state_vertical_   = IDLE_STATE;
+     129             :   States_t current_state_horizontal_  = IDLE_STATE;
+     130             :   States_t previous_state_horizontal_ = IDLE_STATE;
+     131             : 
+     132             :   void changeStateHorizontal(States_t new_state);
+     133             :   void changeStateVertical(States_t new_state);
+     134             :   void changeState(States_t new_state);
+     135             : 
+     136             :   std::atomic<bool> taking_off_ = false;
+     137             :   std::atomic<bool> landing_    = false;
+     138             :   std::atomic<bool> elanding_   = false;
+     139             : 
+     140             :   std::atomic<bool> cause_failsafe_ = false;
+     141             : 
+     142             :   void stopHorizontalMotion(void);
+     143             :   void stopVerticalMotion(void);
+     144             :   void accelerateVertical(void);
+     145             :   void decelerateVertical(void);
+     146             :   void stopHorizontal(void);
+     147             :   void stopVertical(void);
+     148             : 
+     149             :   // | --------------- takeoff / landing services --------------- |
+     150             : 
+     151             :   ros::ServiceServer service_takeoff_;
+     152             :   ros::ServiceServer service_land_;
+     153             :   ros::ServiceServer service_eland_;
+     154             : 
+     155             :   bool callbackTakeoff(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+     156             :   bool callbackLand(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     157             :   bool callbackELand(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     158             : 
+     159             :   // | ------------------ dynamics constraints ------------------ |
+     160             : 
+     161             :   double _horizontal_speed_;
+     162             :   double _vertical_speed_;
+     163             :   double _takeoff_speed_;
+     164             :   double _landing_speed_;
+     165             :   double _elanding_speed_;
+     166             : 
+     167             :   double _horizontal_acceleration_;
+     168             :   double _vertical_acceleration_;
+     169             :   double _takeoff_acceleration_;
+     170             :   double _landing_acceleration_;
+     171             :   double _elanding_acceleration_;
+     172             : 
+     173             :   double _heading_rate_;
+     174             :   double _heading_gain_;
+     175             : 
+     176             :   double _max_position_difference_;
+     177             : 
+     178             :   // | -------------------------- goal -------------------------- |
+     179             : 
+     180             :   double            goal_x_, goal_y_, goal_z_, goal_heading_;
+     181             :   std::atomic<bool> have_goal_ = false;
+     182             :   std::mutex        mutex_goal_;
+     183             : 
+     184             :   // | ---------------- tracker's internal state ---------------- |
+     185             : 
+     186             :   double     state_x_, state_y_, state_z_, state_heading_;
+     187             :   double     speed_x_, speed_y_, speed_heading_;
+     188             :   double     current_heading_, current_vertical_direction_, current_vertical_speed_, current_horizontal_speed_;
+     189             :   double     current_horizontal_acceleration_, current_vertical_acceleration_;
+     190             :   std::mutex mutex_state_;
+     191             : 
+     192             :   // | -------------------- tracker's output -------------------- |
+     193             : 
+     194             :   mrs_msgs::TrackerCommand position_output_;
+     195             : 
+     196             :   // | ------------------------ profiler ------------------------ |
+     197             : 
+     198             :   mrs_lib::Profiler profiler_;
+     199             :   bool              _profiler_enabled_ = false;
+     200             : 
+     201             :   // | ----------------------- constraints ---------------------- |
+     202             : 
+     203             :   mrs_msgs::DynamicsConstraints constraints_;
+     204             :   std::mutex                    mutex_constraints_;
+     205             : };
+     206             : 
+     207             : //}
+     208             : 
+     209             : // | -------------- tracker's interface routines -------------- |
+     210             : 
+     211             : /* //{ initialize() */
+     212             : 
+     213         109 : bool LandoffTracker::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         109 :   this->common_handlers_  = common_handlers;
+     217         109 :   this->private_handlers_ = private_handlers;
+     218             : 
+     219         109 :   _uav_name_ = common_handlers->uav_name;
+     220             : 
+     221         109 :   nh_ = nh;
+     222             : 
+     223         109 :   ros::Time::waitForValid();
+     224             : 
+     225             :   // --------------------------------------------------------------
+     226             :   // |                     loading parameters                     |
+     227             :   // --------------------------------------------------------------
+     228             : 
+     229             :   // | ---------- loading params using the parent's nh ---------- |
+     230             : 
+     231         218 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     232             : 
+     233         109 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     234             : 
+     235         109 :   if (!param_loader_parent.loadedSuccessfully()) {
+     236           0 :     ROS_ERROR("[LandoffTracker]: Could not load all parameters!");
+     237           0 :     return false;
+     238             :   }
+     239             : 
+     240             :   // | --------------- loading plugin's parameters -------------- |
+     241             : 
+     242         109 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/landoff_tracker.yaml");
+     243         109 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/landoff_tracker.yaml");
+     244             : 
+     245         218 :   const std::string yaml_prefix = "mrs_uav_trackers/landoff_tracker/";
+     246             : 
+     247         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "horizontal_tracker/horizontal_speed", _horizontal_speed_);
+     248         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "horizontal_tracker/horizontal_acceleration", _horizontal_acceleration_);
+     249             : 
+     250         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/vertical_speed", _vertical_speed_);
+     251         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/vertical_acceleration", _vertical_acceleration_);
+     252             : 
+     253         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/takeoff_speed", _takeoff_speed_);
+     254         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/takeoff_acceleration", _takeoff_acceleration_);
+     255             : 
+     256         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/landing_speed", _landing_speed_);
+     257         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/landing_acceleration", _landing_acceleration_);
+     258             : 
+     259         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/elanding_speed", _elanding_speed_);
+     260         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/elanding_acceleration", _elanding_acceleration_);
+     261             : 
+     262         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "heading_tracker/heading_rate", _heading_rate_);
+     263         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "heading_tracker/heading_gain", _heading_gain_);
+     264             : 
+     265         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "main_timer_rate", _main_timer_rate_);
+     266             : 
+     267         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "landing_reference", _landing_reference_);
+     268             : 
+     269         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "max_position_difference", _max_position_difference_);
+     270             : 
+     271         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "takeoff_disable_lateral_gains", _takeoff_disable_lateral_gains_);
+     272         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "takeoff_disable_lateral_gains_z", _takeoff_disable_lateral_gains_z_);
+     273             : 
+     274         109 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     275           0 :     ROS_ERROR("[LandoffTracker]: Could not load all parameters!");
+     276           0 :     return false;
+     277             :   }
+     278             : 
+     279         109 :   _tracker_dt_ = 1.0 / double(_main_timer_rate_);
+     280             : 
+     281         109 :   ROS_INFO("[LandoffTracker]: tracker_dt: %f", _tracker_dt_);
+     282             : 
+     283         109 :   state_x_       = 0;
+     284         109 :   state_y_       = 0;
+     285         109 :   state_z_       = 0;
+     286         109 :   state_heading_ = 0;
+     287             : 
+     288         109 :   speed_x_       = 0;
+     289         109 :   speed_y_       = 0;
+     290         109 :   speed_heading_ = 0;
+     291             : 
+     292         109 :   current_horizontal_speed_ = 0;
+     293         109 :   current_vertical_speed_   = 0;
+     294             : 
+     295         109 :   current_horizontal_acceleration_ = 0;
+     296         109 :   current_vertical_acceleration_   = 0;
+     297             : 
+     298         109 :   current_vertical_direction_ = 0;
+     299             : 
+     300         109 :   current_state_vertical_  = LANDED_STATE;
+     301         109 :   previous_state_vertical_ = LANDED_STATE;
+     302             : 
+     303         109 :   current_state_horizontal_  = LANDED_STATE;
+     304         109 :   previous_state_horizontal_ = LANDED_STATE;
+     305             : 
+     306             :   // | ------------------------ profiler ------------------------ |
+     307             : 
+     308         109 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "LandoffTracker", _profiler_enabled_);
+     309             : 
+     310             :   // | ------------------------ services ------------------------ |
+     311             : 
+     312         109 :   service_takeoff_ = nh_.advertiseService("takeoff", &LandoffTracker::callbackTakeoff, this);
+     313         109 :   service_land_    = nh_.advertiseService("land", &LandoffTracker::callbackLand, this);
+     314         109 :   service_eland_   = nh_.advertiseService("eland", &LandoffTracker::callbackELand, this);
+     315             : 
+     316             :   // | ------------------------- timers ------------------------- |
+     317             : 
+     318         109 :   timer_main_ = nh_.createTimer(ros::Rate(_main_timer_rate_), &LandoffTracker::timerMain, this, false, false);
+     319             : 
+     320             :   // | ----------------------- finish init ---------------------- |
+     321             : 
+     322         109 :   is_initialized_ = true;
+     323             : 
+     324         109 :   ROS_INFO("[LandoffTracker]: initialized");
+     325             : 
+     326         109 :   return true;
+     327             : }
+     328             : 
+     329             : //}
+     330             : 
+     331             : /* //{ activate() */
+     332             : 
+     333          46 : std::tuple<bool, std::string> LandoffTracker::activate([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand>& last_tracker_cmd) {
+     334             : 
+     335          92 :   std::stringstream ss;
+     336             : 
+     337          46 :   if (!got_uav_state_) {
+     338             : 
+     339           0 :     ss << "odometry not set";
+     340           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[LandoffTracker]: " << ss.str());
+     341           0 :     return std::tuple(false, ss.str());
+     342             :   }
+     343             : 
+     344          46 :   activate_as_the_first_tracker = !last_tracker_cmd.has_value();
+     345             : 
+     346             :   // copy member variables
+     347          92 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     348             : 
+     349             :   double uav_heading;
+     350             : 
+     351             :   try {
+     352          46 :     uav_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     353             :   }
+     354           0 :   catch (...) {
+     355             : 
+     356           0 :     ss << "could not initialize the UAV heading";
+     357           0 :     ROS_ERROR_STREAM("[LandoffTracker]: " << ss.str());
+     358           0 :     return std::tuple(false, ss.str());
+     359             :   }
+     360             : 
+     361             :   // --------------------------------------------------------------
+     362             :   // |                      initial condition                     |
+     363             :   // --------------------------------------------------------------
+     364             : 
+     365             :   {
+     366          92 :     std::scoped_lock lock(mutex_goal_);
+     367             : 
+     368             :     // the last command is usable
+     369          46 :     state_x_       = uav_state.pose.position.x;
+     370          46 :     state_y_       = uav_state.pose.position.y;
+     371          46 :     state_z_       = uav_state.pose.position.z;
+     372          46 :     state_heading_ = uav_heading;
+     373             : 
+     374          46 :     speed_x_         = uav_state.velocity.linear.x;
+     375          46 :     speed_y_         = uav_state.velocity.linear.y;
+     376          46 :     current_heading_ = atan2(speed_y_, speed_x_);
+     377             : 
+     378          46 :     current_horizontal_speed_ = sqrt(pow(speed_x_, 2) + pow(speed_y_, 2));
+     379             : 
+     380          46 :     current_vertical_speed_     = fabs(uav_state.velocity.linear.z);
+     381          46 :     current_vertical_direction_ = uav_state.velocity.linear.z > 0 ? +1 : -1;
+     382             : 
+     383          46 :     current_horizontal_acceleration_ = 0;
+     384          46 :     current_vertical_acceleration_   = 0;
+     385             : 
+     386          46 :     goal_heading_ = uav_heading;
+     387             : 
+     388          46 :     ROS_INFO("[LandoffTracker]: initial condition: x: %.2f, y: %.2f, z: %.2f, heading: %.2f", state_x_, state_y_, state_z_, state_heading_);
+     389             :   }
+     390             : 
+     391             :   // --------------------------------------------------------------
+     392             :   // |          horizontal initial conditions prediction          |
+     393             :   // --------------------------------------------------------------
+     394             : 
+     395             :   double horizontal_t_stop, horizontal_stop_dist, stop_dist_x, stop_dist_y;
+     396             : 
+     397             :   {
+     398          46 :     std::scoped_lock lock(mutex_state_);
+     399             : 
+     400          46 :     horizontal_t_stop    = current_horizontal_speed_ / _horizontal_acceleration_;
+     401          46 :     horizontal_stop_dist = (horizontal_t_stop * current_horizontal_speed_) / 2.0;
+     402          46 :     stop_dist_x          = cos(current_heading_) * horizontal_stop_dist;
+     403          46 :     stop_dist_y          = sin(current_heading_) * horizontal_stop_dist;
+     404             :   }
+     405             : 
+     406             :   // --------------------------------------------------------------
+     407             :   // |           vertical initial conditions prediction           |
+     408             :   // --------------------------------------------------------------
+     409             : 
+     410             :   double vertical_t_stop, vertical_stop_dist;
+     411             : 
+     412             :   {
+     413          46 :     std::scoped_lock lock(mutex_state_);
+     414             : 
+     415          46 :     vertical_t_stop    = current_vertical_speed_ / _vertical_acceleration_;
+     416          46 :     vertical_stop_dist = current_vertical_direction_ * (vertical_t_stop * current_vertical_speed_) / 2.0;
+     417             :   }
+     418             : 
+     419             :   // --------------------------------------------------------------
+     420             :   // |               heading initial condition prediction             |
+     421             :   // --------------------------------------------------------------
+     422             : 
+     423             :   {
+     424          46 :     std::scoped_lock lock(mutex_goal_, mutex_state_);
+     425             : 
+     426          46 :     goal_x_ = state_x_ + stop_dist_x;
+     427          46 :     goal_y_ = state_y_ + stop_dist_y;
+     428          46 :     goal_z_ = state_z_ + vertical_stop_dist;
+     429             :   }
+     430             : 
+     431          46 :   landing_        = false;
+     432          46 :   taking_off_     = false;
+     433          46 :   is_active_      = true;
+     434          46 :   have_goal_      = false;
+     435          46 :   cause_failsafe_ = false;
+     436             : 
+     437          46 :   timer_main_.start();
+     438             : 
+     439             :   {
+     440          92 :     std::scoped_lock lock(mutex_goal_);
+     441             : 
+     442          46 :     ROS_INFO("[LandoffTracker]: stopping goal: x: %.2f, y: %.2f, z: %.2f, heading: %.2f", goal_x_, goal_y_, goal_z_, goal_heading_);
+     443             :   }
+     444             : 
+     445          46 :   changeState(STOP_MOTION_STATE);
+     446             : 
+     447          46 :   ss << "activated";
+     448          46 :   ROS_INFO_STREAM("[LandoffTracker]: " << ss.str());
+     449             : 
+     450          46 :   return std::tuple(true, ss.str());
+     451             : }
+     452             : 
+     453             : //}
+     454             : 
+     455             : /* //{ deactivate() */
+     456             : 
+     457          62 : void LandoffTracker::deactivate(void) {
+     458             : 
+     459          62 :   is_active_                = false;
+     460          62 :   landing_                  = false;
+     461          62 :   taking_off_               = false;
+     462          62 :   current_state_vertical_   = IDLE_STATE;
+     463          62 :   current_state_horizontal_ = IDLE_STATE;
+     464             : 
+     465          62 :   timer_main_.stop();
+     466             : 
+     467          62 :   ROS_INFO("[LandoffTracker]: deactivated");
+     468          62 : }
+     469             : 
+     470             : //}
+     471             : 
+     472             : /* //{ resetStatic() */
+     473             : 
+     474           0 : bool LandoffTracker::resetStatic(void) {
+     475             : 
+     476           0 :   return false;
+     477             : }
+     478             : 
+     479             : //}
+     480             : 
+     481             : /* //{ update() */
+     482             : 
+     483      142109 : std::optional<mrs_msgs::TrackerCommand> LandoffTracker::update(const mrs_msgs::UavState&                                           uav_state,
+     484             :                                                                [[maybe_unused]] const mrs_uav_managers::Controller::ControlOutput& last_control_output) {
+     485             : 
+     486      426327 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     487      426327 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("LandoffTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     488             : 
+     489             :   {
+     490      142109 :     std::scoped_lock lock(mutex_uav_state_);
+     491             : 
+     492      142109 :     uav_state_ = uav_state;
+     493             : 
+     494      142109 :     got_uav_state_ = true;
+     495             :   }
+     496             : 
+     497             :   // up to this part the update() method is evaluated even when the tracker is not active
+     498      142109 :   if (!is_active_ || cause_failsafe_) {
+     499      123920 :     return {};
+     500             :   }
+     501             : 
+     502       18189 :   position_output_.header.stamp    = ros::Time::now();
+     503       18189 :   position_output_.header.frame_id = uav_state_.header.frame_id;
+     504             : 
+     505             :   {
+     506       18189 :     std::scoped_lock lock(mutex_state_);
+     507             : 
+     508       18189 :     position_output_.position.x = state_x_;
+     509       18189 :     position_output_.position.y = state_y_;
+     510       18189 :     position_output_.position.z = state_z_;
+     511       18189 :     position_output_.heading    = state_heading_;
+     512             : 
+     513       18189 :     position_output_.velocity.x   = cos(current_heading_) * current_horizontal_speed_;
+     514       18189 :     position_output_.velocity.y   = sin(current_heading_) * current_horizontal_speed_;
+     515       18189 :     position_output_.velocity.z   = current_vertical_direction_ * current_vertical_speed_;
+     516       18189 :     position_output_.heading_rate = speed_heading_;
+     517             : 
+     518       18189 :     position_output_.use_position_vertical   = 1;
+     519       18189 :     position_output_.use_position_horizontal = 1;
+     520       18189 :     position_output_.use_heading             = 1;
+     521       18189 :     position_output_.use_heading_rate        = 1;
+     522       18189 :     position_output_.use_velocity_vertical   = 1;
+     523       18189 :     position_output_.use_velocity_horizontal = 1;
+     524             :   }
+     525             : 
+     526             :   {
+     527       36378 :     std::scoped_lock lock(mutex_last_control_output_);
+     528             : 
+     529       18189 :     last_control_output_ = last_control_output;
+     530             :   }
+     531             : 
+     532       18189 :   if (_takeoff_disable_lateral_gains_ && taking_off_ && uav_state_.pose.position.z < _takeoff_disable_lateral_gains_z_) {
+     533           0 :     position_output_.disable_position_gains = true;
+     534             :   } else {
+     535       18189 :     position_output_.disable_position_gains = false;
+     536             :   }
+     537             : 
+     538       18189 :   if (taking_off_) {
+     539        9371 :     position_output_.disable_antiwindups = true;
+     540             :   } else {
+     541        8818 :     position_output_.disable_antiwindups = false;
+     542             :   }
+     543             : 
+     544       18189 :   return {position_output_};
+     545             : }
+     546             : 
+     547             : //}
+     548             : 
+     549             : /* //{ getStatus() */
+     550             : 
+     551        2288 : const mrs_msgs::TrackerStatus LandoffTracker::getStatus() {
+     552             : 
+     553        2288 :   mrs_msgs::TrackerStatus tracker_status;
+     554             : 
+     555        2288 :   tracker_status.active            = is_active_;
+     556        2288 :   tracker_status.callbacks_enabled = callbacks_enabled_;
+     557             : 
+     558        2288 :   const bool hovering = current_state_vertical_ == HOVER_STATE && current_state_horizontal_ == HOVER_STATE;
+     559        2288 :   const bool idling   = current_state_vertical_ == IDLE_STATE && current_state_horizontal_ == IDLE_STATE;
+     560             : 
+     561             :   // the (hovering && activate_as_the_first_tracker) part of the condition makes sure that the IDLE will be
+     562             :   // reported even when hovering on the ground, just before the takeoff service is called
+     563        2288 :   if (idling || (hovering && activate_as_the_first_tracker)) {
+     564             : 
+     565         184 :     tracker_status.state = mrs_msgs::TrackerStatus::STATE_IDLE;
+     566             : 
+     567        2104 :   } else if (taking_off_) {
+     568             : 
+     569        1017 :     tracker_status.state = mrs_msgs::TrackerStatus::STATE_TAKEOFF;
+     570             : 
+     571        1087 :   } else if (hovering) {
+     572             : 
+     573           0 :     tracker_status.state = mrs_msgs::TrackerStatus::STATE_HOVER;
+     574             : 
+     575        1087 :   } else if (landing_) {
+     576             : 
+     577        1082 :     tracker_status.state = mrs_msgs::TrackerStatus::STATE_LAND;
+     578             :   }
+     579             : 
+     580        2288 :   tracker_status.have_goal = landing_ || taking_off_ || !(hovering || idling);
+     581             : 
+     582        2288 :   tracker_status.tracking_trajectory = false;
+     583             : 
+     584        2288 :   return tracker_status;
+     585             : }
+     586             : 
+     587             : //}
+     588             : 
+     589             : /* //{ enableCallbacks() */
+     590             : 
+     591         312 : const std_srvs::SetBoolResponse::ConstPtr LandoffTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr& cmd) {
+     592             : 
+     593         624 :   std_srvs::SetBoolResponse res;
+     594         624 :   std::stringstream         ss;
+     595             : 
+     596         312 :   if (cmd->data != callbacks_enabled_) {
+     597             : 
+     598          20 :     callbacks_enabled_ = cmd->data;
+     599             : 
+     600          20 :     ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+     601          20 :     ROS_INFO_STREAM_THROTTLE(1.0, "[LandoffTrakcer]: " << ss.str());
+     602             : 
+     603             :   } else {
+     604             : 
+     605         292 :     ss << "callbacks were already " << (callbacks_enabled_ ? "enabled" : "disabled");
+     606         292 :     ROS_WARN_STREAM_THROTTLE(1.0, "[LandoffTrakcer]: " << ss.str());
+     607             :   }
+     608             : 
+     609         312 :   res.message = ss.str();
+     610         312 :   res.success = true;
+     611             : 
+     612         624 :   return std_srvs::SetBoolResponse::ConstPtr(new std_srvs::SetBoolResponse(res));
+     613             : }
+     614             : 
+     615             : //}
+     616             : 
+     617             : /* switchOdometrySource() //{ */
+     618             : 
+     619           0 : const std_srvs::TriggerResponse::ConstPtr LandoffTracker::switchOdometrySource([[maybe_unused]] const mrs_msgs::UavState& new_uav_state) {
+     620             : 
+     621           0 :   std::scoped_lock lock(mutex_goal_, mutex_state_);
+     622             : 
+     623           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     624             : 
+     625           0 :   double old_heading  = 0;
+     626           0 :   double new_heading  = 0;
+     627           0 :   bool   got_headings = true;
+     628             : 
+     629             :   try {
+     630           0 :     old_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     631             :   }
+     632           0 :   catch (...) {
+     633           0 :     ROS_ERROR_THROTTLE(1.0, "[LandoffTracker]: could not calculate the old UAV heading");
+     634           0 :     got_headings = false;
+     635             :   }
+     636             : 
+     637             :   try {
+     638           0 :     new_heading = mrs_lib::AttitudeConverter(new_uav_state.pose.orientation).getHeading();
+     639             :   }
+     640           0 :   catch (...) {
+     641           0 :     ROS_ERROR_THROTTLE(1.0, "[LandoffTracker]: could not calculate the new UAV heading");
+     642           0 :     got_headings = false;
+     643             :   }
+     644             : 
+     645           0 :   std_srvs::TriggerResponse res;
+     646             : 
+     647           0 :   if (!got_headings) {
+     648           0 :     res.message = "could not calculate the heading difference";
+     649           0 :     res.success = false;
+     650             : 
+     651           0 :     return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+     652             :   }
+     653             : 
+     654             :   // | --------- recalculate the goal to new coordinates -------- |
+     655             : 
+     656           0 :   double dx       = new_uav_state.pose.position.x - uav_state.pose.position.x;
+     657           0 :   double dy       = new_uav_state.pose.position.y - uav_state.pose.position.y;
+     658           0 :   double dz       = new_uav_state.pose.position.z - uav_state.pose.position.z;
+     659           0 :   double dheading = new_heading - old_heading;
+     660             : 
+     661           0 :   goal_x_ += dx;
+     662           0 :   goal_y_ += dy;
+     663           0 :   goal_z_ += dz;
+     664           0 :   goal_heading_ += dheading;
+     665             : 
+     666             :   // | -------------------- update the state -------------------- |
+     667             : 
+     668           0 :   state_x_ += dx;
+     669           0 :   state_y_ += dy;
+     670           0 :   state_z_ += dz;
+     671           0 :   state_heading_ += dheading;
+     672             : 
+     673           0 :   current_heading_ = atan2(goal_y_ - state_y_, goal_x_ - state_x_);
+     674             : 
+     675           0 :   res.message = "odometry source switched";
+     676           0 :   res.success = true;
+     677             : 
+     678           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+     679             : }
+     680             : 
+     681             : //}
+     682             : 
+     683             : /* //{ hover() */
+     684           0 : const std_srvs::TriggerResponse::ConstPtr LandoffTracker::hover([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+     685             : 
+     686           0 :   std::scoped_lock lock(mutex_main_timer_);
+     687             : 
+     688             :   // copy member variables
+     689           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     690             : 
+     691           0 :   auto [current_horizontal_speed, current_vertical_speed, current_heading, current_vertical_direction] =
+     692           0 :       mrs_lib::get_mutexed(mutex_state_, current_horizontal_speed_, current_vertical_speed_, current_heading_, current_vertical_direction_);
+     693             : 
+     694           0 :   std_srvs::TriggerResponse res;
+     695             : 
+     696             :   // --------------------------------------------------------------
+     697             :   // |          horizontal initial conditions prediction          |
+     698             :   // --------------------------------------------------------------
+     699             :   {
+     700           0 :     std::scoped_lock lock(mutex_state_);
+     701             : 
+     702           0 :     current_horizontal_speed_ = sqrt(pow(uav_state.velocity.linear.x, 2) + pow(uav_state.velocity.linear.y, 2));
+     703           0 :     current_vertical_speed_   = uav_state.velocity.linear.z;
+     704           0 :     current_heading_          = atan2(uav_state.velocity.linear.y, uav_state.velocity.linear.x);
+     705             :   }
+     706             : 
+     707             :   double horizontal_t_stop, horizontal_stop_dist, stop_dist_x, stop_dist_y;
+     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             :   // |           vertical initial conditions prediction           |
+     716             :   // --------------------------------------------------------------
+     717             : 
+     718           0 :   double vertical_t_stop    = current_vertical_speed / _vertical_acceleration_;
+     719           0 :   double vertical_stop_dist = current_vertical_direction * (vertical_t_stop * current_vertical_speed) / 2.0;
+     720             : 
+     721             :   // --------------------------------------------------------------
+     722             :   // |                        set the goal                        |
+     723             :   // --------------------------------------------------------------
+     724             : 
+     725             :   {
+     726           0 :     std::scoped_lock lock(mutex_state_, mutex_goal_);
+     727             : 
+     728           0 :     goal_x_ = state_x_ + stop_dist_x;
+     729           0 :     goal_y_ = state_y_ + stop_dist_y;
+     730           0 :     goal_z_ = state_z_ + vertical_stop_dist;
+     731             :   }
+     732             : 
+     733           0 :   res.message = "hover initiated";
+     734           0 :   res.success = true;
+     735             : 
+     736           0 :   changeState(STOP_MOTION_STATE);
+     737             : 
+     738           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+     739             : }
+     740             : 
+     741             : //}
+     742             : 
+     743             : /* //{ startTrajectoryTracking() */
+     744             : 
+     745           0 : const std_srvs::TriggerResponse::ConstPtr LandoffTracker::startTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+     746           0 :   return std_srvs::TriggerResponse::Ptr();
+     747             : }
+     748             : 
+     749             : //}
+     750             : 
+     751             : /* //{ stopTrajectoryTracking() */
+     752             : 
+     753           0 : const std_srvs::TriggerResponse::ConstPtr LandoffTracker::stopTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+     754           0 :   return std_srvs::TriggerResponse::Ptr();
+     755             : }
+     756             : 
+     757             : //}
+     758             : 
+     759             : /* //{ resumeTrajectoryTracking() */
+     760             : 
+     761           0 : const std_srvs::TriggerResponse::ConstPtr LandoffTracker::resumeTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+     762           0 :   return std_srvs::TriggerResponse::Ptr();
+     763             : }
+     764             : 
+     765             : //}
+     766             : 
+     767             : /* //{ gotoTrajectoryStart() */
+     768             : 
+     769           0 : const std_srvs::TriggerResponse::ConstPtr LandoffTracker::gotoTrajectoryStart([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+     770           0 :   return std_srvs::TriggerResponse::Ptr();
+     771             : }
+     772             : 
+     773             : //}
+     774             : 
+     775             : /* //{ setConstraints() */
+     776             : 
+     777         387 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr LandoffTracker::setConstraints([
+     778             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& cmd) {
+     779             : 
+     780             : 
+     781         387 :   mrs_lib::set_mutexed(mutex_constraints_, cmd->constraints, constraints_);
+     782             : 
+     783         387 :   ROS_INFO("[LandoffTracker]: updating constraints");
+     784             : 
+     785         774 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     786         387 :   res.success = true;
+     787         387 :   res.message = "constraints updated";
+     788             : 
+     789         774 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     790             : }
+     791             : 
+     792             : //}
+     793             : 
+     794             : /* //{ setReference() */
+     795             : 
+     796           0 : const mrs_msgs::ReferenceSrvResponse::ConstPtr LandoffTracker::setReference([[maybe_unused]] const mrs_msgs::ReferenceSrvRequest::ConstPtr& cmd) {
+     797             : 
+     798           0 :   return mrs_msgs::ReferenceSrvResponse::Ptr();
+     799             : }
+     800             : 
+     801             : //}
+     802             : 
+     803             : /* //{ setVelocityReference() */
+     804             : 
+     805           0 : const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr LandoffTracker::setVelocityReference([
+     806             :     [maybe_unused]] const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr& cmd) {
+     807           0 :   return mrs_msgs::VelocityReferenceSrvResponse::Ptr();
+     808             : }
+     809             : 
+     810             : //}
+     811             : 
+     812             : /* //{ setTrajectoryReference() */
+     813             : 
+     814           6 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr LandoffTracker::setTrajectoryReference([
+     815             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr& cmd) {
+     816           6 :   return mrs_msgs::TrajectoryReferenceSrvResponse::Ptr();
+     817             : }
+     818             : 
+     819             : //}
+     820             : 
+     821             : // | ----------------- state machine routines ----------------- |
+     822             : 
+     823             : /* //{ changeStateHorizontal() */
+     824             : 
+     825         185 : void LandoffTracker::changeStateHorizontal(States_t new_state) {
+     826             : 
+     827         185 :   previous_state_horizontal_ = current_state_horizontal_;
+     828         185 :   current_state_horizontal_  = new_state;
+     829             : 
+     830         185 :   switch (current_state_horizontal_) {
+     831             : 
+     832          59 :     case STOPPING_STATE: {
+     833             : 
+     834         118 :       std::scoped_lock lock(mutex_state_);
+     835          59 :       current_horizontal_speed_ = 0;
+     836             : 
+     837          59 :       break;
+     838             :     };
+     839             : 
+     840         126 :     default: {
+     841             : 
+     842         126 :       break;
+     843             :     }
+     844             :   }
+     845             : 
+     846         185 :   ROS_INFO("[LandoffTracker]: Switching horizontal state %s -> %s", state_names.at(previous_state_horizontal_), state_names.at(current_state_horizontal_));
+     847         185 : }
+     848             : 
+     849             : //}
+     850             : 
+     851             : /* //{ changeStateVertical() */
+     852             : 
+     853         227 : void LandoffTracker::changeStateVertical(States_t new_state) {
+     854             : 
+     855         227 :   previous_state_vertical_ = current_state_vertical_;
+     856         227 :   current_state_vertical_  = new_state;
+     857             : 
+     858         227 :   switch (current_state_vertical_) {
+     859             : 
+     860          46 :     case HOVER_STATE: {
+     861          46 :       taking_off_ = false;
+     862          46 :       break;
+     863             :     }
+     864             : 
+     865         181 :     default: {
+     866         181 :       break;
+     867             :     }
+     868             :   }
+     869             : 
+     870         227 :   ROS_INFO("[LandoffTracker]: Switching vertical state %s -> %s", state_names.at(previous_state_vertical_), state_names.at(current_state_vertical_));
+     871         227 : }
+     872             : 
+     873             : //}
+     874             : 
+     875             : /* //{ changeState() */
+     876             : 
+     877         151 : void LandoffTracker::changeState(States_t new_state) {
+     878             : 
+     879         151 :   changeStateVertical(new_state);
+     880         151 :   changeStateHorizontal(new_state);
+     881         151 : }
+     882             : 
+     883             : //}
+     884             : 
+     885             : // | --------------------- motion routines -------------------- |
+     886             : 
+     887             : /* //{ stopHorizontalMotion() */
+     888             : 
+     889         682 : void LandoffTracker::stopHorizontalMotion(void) {
+     890             : 
+     891             :   {
+     892        1364 :     std::scoped_lock lock(mutex_state_);
+     893             : 
+     894         682 :     current_horizontal_speed_ -= _horizontal_acceleration_ * _tracker_dt_;
+     895             : 
+     896         682 :     if (current_horizontal_speed_ < 0) {
+     897         421 :       current_horizontal_speed_        = 0;
+     898         421 :       current_horizontal_acceleration_ = 0;
+     899             :     } else {
+     900         261 :       current_horizontal_acceleration_ = -_horizontal_acceleration_;
+     901             :     }
+     902             :   }
+     903         682 : }
+     904             : 
+     905             : //}
+     906             : 
+     907             : /* //{ stopVerticalMotion() */
+     908             : 
+     909         682 : void LandoffTracker::stopVerticalMotion(void) {
+     910             : 
+     911             :   {
+     912        1364 :     std::scoped_lock lock(mutex_state_);
+     913             : 
+     914         682 :     current_vertical_speed_ -= _vertical_acceleration_ * _tracker_dt_;
+     915             : 
+     916         682 :     if (current_vertical_speed_ < 0) {
+     917          35 :       current_vertical_speed_        = 0;
+     918          35 :       current_vertical_acceleration_ = 0;
+     919             :     } else {
+     920         647 :       current_vertical_acceleration_ = -_vertical_acceleration_;
+     921             :     }
+     922             :   }
+     923         682 : }
+     924             : 
+     925             : //}
+     926             : 
+     927             : /* //{ accelerateVertical() */
+     928             : 
+     929       16042 : void LandoffTracker::accelerateVertical(void) {
+     930             : 
+     931             :   // copy member variables
+     932       16042 :   auto [current_vertical_speed, state_z] = mrs_lib::get_mutexed(mutex_state_, current_vertical_speed_, state_z_);
+     933       16042 :   auto goal_z                            = mrs_lib::get_mutexed(mutex_goal_, goal_z_);
+     934       16042 :   auto constraints                       = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     935             : 
+     936             :   double used_acceleration;
+     937             :   double used_speed;
+     938             : 
+     939       16042 :   if (taking_off_) {
+     940             : 
+     941        4797 :     used_speed        = _takeoff_speed_;
+     942        4797 :     used_acceleration = _takeoff_acceleration_;
+     943             : 
+     944        4797 :     if (used_speed > constraints.vertical_ascending_speed) {
+     945           0 :       used_speed = constraints.vertical_ascending_speed;
+     946           0 :       ROS_WARN_THROTTLE(1.0, "[LandoffTracker]: saturating takeoff speed");
+     947             :     }
+     948             : 
+     949        4797 :     if (used_acceleration > constraints.vertical_ascending_acceleration) {
+     950           0 :       used_acceleration = constraints.vertical_ascending_acceleration;
+     951           0 :       ROS_WARN_THROTTLE(1.0, "[LandoffTracker]: saturating takeoff acceleration");
+     952             :     }
+     953             : 
+     954       11245 :   } else if (landing_) {
+     955             : 
+     956       11245 :     if (elanding_) {
+     957             : 
+     958        7014 :       used_speed        = _elanding_speed_;
+     959        7014 :       used_acceleration = _elanding_acceleration_;
+     960             : 
+     961             :     } else {
+     962             : 
+     963        4231 :       used_speed        = _landing_speed_;
+     964        4231 :       used_acceleration = _landing_acceleration_;
+     965             : 
+     966        4231 :       if (used_speed > constraints.vertical_descending_speed) {
+     967           0 :         used_speed = constraints.vertical_descending_speed;
+     968           0 :         ROS_WARN_THROTTLE(1.0, "[LandoffTracker]: saturating landing speed");
+     969             :       }
+     970             : 
+     971        4231 :       if (used_acceleration > constraints.vertical_descending_acceleration) {
+     972           0 :         used_acceleration = constraints.vertical_descending_acceleration;
+     973           0 :         ROS_WARN_THROTTLE(1.0, "[LandoffTracker]: saturating landing acceleration");
+     974             :       }
+     975             :     }
+     976             : 
+     977             :   } else {
+     978             : 
+     979             :     // TODO take this from constraints
+     980           0 :     used_speed        = _vertical_speed_;
+     981           0 :     used_acceleration = _vertical_acceleration_;
+     982             :   }
+     983             : 
+     984             :   // set the right heading
+     985       16042 :   double tar_z = goal_z - state_z;
+     986             : 
+     987             :   // set the right vertical direction
+     988             :   {
+     989       16042 :     std::scoped_lock lock(mutex_state_);
+     990             : 
+     991       16042 :     current_vertical_direction_ = mrs_lib::signum(tar_z);
+     992             :   }
+     993             : 
+     994       16042 :   auto current_vertical_direction = mrs_lib::get_mutexed(mutex_state_, current_vertical_direction_);
+     995             : 
+     996             :   // calculate the time to stop and the distance it will take to stop [vertical]
+     997       16042 :   double vertical_t_stop    = current_vertical_speed / used_acceleration;
+     998       16042 :   double vertical_stop_dist = (vertical_t_stop * current_vertical_speed) / 2.0;
+     999       16042 :   double stop_dist_z        = current_vertical_direction * vertical_stop_dist;
+    1000             : 
+    1001             :   {
+    1002       32084 :     std::scoped_lock lock(mutex_state_);
+    1003             : 
+    1004       16042 :     current_vertical_speed_ += used_acceleration * _tracker_dt_;
+    1005             : 
+    1006       16042 :     if (current_vertical_speed_ >= used_speed) {
+    1007        3854 :       current_vertical_speed_ -= _vertical_acceleration_ * _tracker_dt_;
+    1008        3854 :       current_vertical_acceleration_ = 0;
+    1009             :     } else {
+    1010       12188 :       current_vertical_acceleration_ = used_acceleration;
+    1011             :     }
+    1012             :   }
+    1013             : 
+    1014             :   // stopping condition to change to decelerate state
+    1015             :   //
+    1016             :   // It does not apply if landing or elanding, cause,
+    1017             :   // it could potentially stop in mid air if odometry jumps (this happened),
+    1018             :   // Instead, landing and elanding is stopped by sensing the throttle.
+    1019       16042 :   if (!elanding_ && !landing_) {
+    1020        4797 :     if (fabs(state_z + stop_dist_z - goal_z) < (2 * (used_speed * _tracker_dt_))) {
+    1021             : 
+    1022             :       {
+    1023          21 :         std::scoped_lock lock(mutex_state_);
+    1024             : 
+    1025          21 :         current_vertical_acceleration_ = 0;
+    1026             :       }
+    1027             : 
+    1028          21 :       changeStateVertical(DECELERATING_STATE);
+    1029             :     }
+    1030             :   }
+    1031       16042 : }
+    1032             : 
+    1033             : //}
+    1034             : 
+    1035             : /* //{ decelerateVertical() */
+    1036             : 
+    1037        4797 : void LandoffTracker::decelerateVertical(void) {
+    1038             : 
+    1039             :   double used_acceleration;
+    1040             : 
+    1041        4797 :   if (taking_off_) {
+    1042             : 
+    1043        4797 :     used_acceleration = _takeoff_acceleration_;
+    1044             : 
+    1045           0 :   } else if (landing_) {
+    1046             : 
+    1047           0 :     if (elanding_) {
+    1048             : 
+    1049           0 :       used_acceleration = _elanding_acceleration_;
+    1050             : 
+    1051             :     } else {
+    1052             : 
+    1053           0 :       used_acceleration = _landing_acceleration_;
+    1054             :     }
+    1055             : 
+    1056             :   } else {
+    1057           0 :     used_acceleration = _vertical_acceleration_;
+    1058             :   }
+    1059             : 
+    1060             :   {
+    1061        9594 :     std::scoped_lock lock(mutex_state_);
+    1062             : 
+    1063        4797 :     current_vertical_speed_ -= used_acceleration * _tracker_dt_;
+    1064             : 
+    1065        4797 :     if (current_vertical_speed_ < 0) {
+    1066          21 :       current_vertical_speed_ = 0;
+    1067             :     } else {
+    1068        4776 :       current_vertical_acceleration_ = -used_acceleration;
+    1069             :     }
+    1070             :   }
+    1071             : 
+    1072        4797 :   auto current_vertical_speed = mrs_lib::get_mutexed(mutex_state_, current_vertical_speed_);
+    1073             : 
+    1074        4797 :   if (current_vertical_speed == 0) {
+    1075             : 
+    1076             :     {
+    1077          21 :       std::scoped_lock lock(mutex_state_);
+    1078             : 
+    1079          21 :       current_vertical_acceleration_ = 0;
+    1080             :     }
+    1081             : 
+    1082          21 :     changeStateVertical(STOPPING_STATE);
+    1083             :   }
+    1084        4797 : }
+    1085             : 
+    1086             : //}
+    1087             : 
+    1088             : /* //{ stopHorizontal() */
+    1089             : 
+    1090       20839 : void LandoffTracker::stopHorizontal(void) {
+    1091             : 
+    1092             :   {
+    1093       20839 :     std::scoped_lock lock(mutex_state_, mutex_goal_);
+    1094             : 
+    1095       20839 :     double new_state_x = 0.95 * state_x_ + 0.05 * goal_x_;
+    1096       20839 :     double new_state_y = 0.95 * state_y_ + 0.05 * goal_y_;
+    1097             : 
+    1098       20839 :     double dist_x = new_state_x - state_x_;
+    1099       20839 :     double dist_y = new_state_y - state_y_;
+    1100             : 
+    1101       20839 :     double dt = 1.0 / _main_timer_rate_;
+    1102             : 
+    1103       20839 :     if (std::abs(dist_x / dt) > 1.0) {
+    1104           0 :       dist_x = mrs_lib::signum(dist_x) * (1.0 * dt);
+    1105             :     }
+    1106             : 
+    1107       20839 :     if (std::abs(dist_y / dt) > 1.0) {
+    1108           0 :       dist_y = mrs_lib::signum(dist_y) * (1.0 * dt);
+    1109             :     }
+    1110             : 
+    1111       20839 :     state_x_ += dist_x;
+    1112       20839 :     state_y_ += dist_y;
+    1113             : 
+    1114       20839 :     current_horizontal_acceleration_ = 0;
+    1115             :   }
+    1116       20839 : }
+    1117             : 
+    1118             : //}
+    1119             : 
+    1120             : /* //{ stopVertical() */
+    1121             : 
+    1122           0 : void LandoffTracker::stopVertical(void) {
+    1123             : 
+    1124             :   {
+    1125           0 :     std::scoped_lock lock(mutex_state_, mutex_goal_);
+    1126             : 
+    1127           0 :     double new_state_z = 0.95 * state_z_ + 0.05 * goal_z_;
+    1128             : 
+    1129           0 :     double dist_z = new_state_z - state_z_;
+    1130             : 
+    1131           0 :     double dt = 1.0 / _main_timer_rate_;
+    1132             : 
+    1133           0 :     if (std::abs(dist_z / dt) > 1.0) {
+    1134           0 :       dist_z = mrs_lib::signum(dist_z) * (1.0 * dt);
+    1135             :     }
+    1136             : 
+    1137           0 :     state_z_ += dist_z;
+    1138             : 
+    1139           0 :     current_vertical_acceleration_ = 0;
+    1140             :   }
+    1141           0 : }
+    1142             : 
+    1143             : //}
+    1144             : 
+    1145             : // | --------------------- timer routines --------------------- |
+    1146             : 
+    1147             : /* //{ timerMain() */
+    1148             : 
+    1149       23912 : void LandoffTracker::timerMain(const ros::TimerEvent& event) {
+    1150             : 
+    1151       23912 :   std::scoped_lock lock(mutex_main_timer_);
+    1152             : 
+    1153       23912 :   if (!is_active_) {
+    1154           0 :     return;
+    1155             :   }
+    1156             : 
+    1157             :   // copy member variables
+    1158       47824 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1159       23912 :   auto [state_x, state_y, state_z, current_horizontal_speed, current_vertical_speed, current_heading, current_vertical_direction] = mrs_lib::get_mutexed(
+    1160       23912 :       mutex_state_, state_x_, state_y_, state_z_, current_horizontal_speed_, current_vertical_speed_, current_heading_, current_vertical_direction_);
+    1161       23912 :   auto [goal_x, goal_y, goal_z] = mrs_lib::get_mutexed(mutex_goal_, goal_x_, goal_y_, goal_z_);
+    1162       47824 :   auto last_control_output      = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    1163             : 
+    1164             :   double uav_x, uav_y, uav_z;
+    1165       23912 :   uav_x = uav_state.pose.position.x;
+    1166       23912 :   uav_y = uav_state.pose.position.y;
+    1167       23912 :   uav_z = uav_state.pose.position.z;
+    1168             : 
+    1169       71736 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("main", _main_timer_rate_, 0.002, event);
+    1170       71736 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("LandoffTracker::main", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1171             : 
+    1172       23912 :   bool takeoff_saturated = false;
+    1173             : 
+    1174       23912 :   if (taking_off_) {
+    1175             : 
+    1176             :     // calculate the vector
+    1177       10170 :     double err_x      = uav_x - state_x;
+    1178       10170 :     double err_y      = uav_y - state_y;
+    1179       10170 :     double err_z      = uav_z - state_z;
+    1180       10170 :     double error_size = sqrt(pow(err_x, 2) + pow(err_y, 2) + pow(err_z, 2));
+    1181             : 
+    1182       10170 :     if (error_size > _max_position_difference_) {
+    1183             : 
+    1184             :       // calculate the potential next step
+    1185         128 :       double future_state_x = state_x + cos(current_heading) * current_horizontal_speed * _tracker_dt_;
+    1186         128 :       double future_state_y = state_y + sin(current_heading) * current_horizontal_speed * _tracker_dt_;
+    1187         128 :       double future_state_z = state_z + current_vertical_direction * current_vertical_speed * _tracker_dt_;
+    1188             : 
+    1189             :       // if the step would lead to a greater control error than the threshold
+    1190         128 :       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) {
+    1191             : 
+    1192             :         // set this to true... later, we will not update the model if this is true, thus the tracker's motion will stop
+    1193             :         // => the tracker will wait for the controller
+    1194         128 :         takeoff_saturated = true;
+    1195             : 
+    1196         128 :         ROS_WARN_THROTTLE(
+    1197             :             0.1, "[LandoffTracker]: position difference %.3f > %.3f, saturating the motion. Reference: x=%.2f, y=%.2f, z=%.2f, Odometry: %.2f, %.2f, %.2f",
+    1198             :             error_size, _max_position_difference_, future_state_x, future_state_y, future_state_z, uav_x, uav_y, uav_z);
+    1199             :       }
+    1200             :     }
+    1201             : 
+    1202             :     // saturate while ramping up during takeoff
+    1203       10170 :     if (last_control_output.diagnostics.ramping_up) {
+    1204             : 
+    1205         447 :       ROS_INFO_THROTTLE(1.0, "[LandoffTracker]: waiting for the controller to rampup");
+    1206         447 :       takeoff_saturated = true;
+    1207             :     }
+    1208             :   }
+    1209             : 
+    1210       23912 :   if (!takeoff_saturated) {
+    1211             : 
+    1212       23337 :     switch (current_state_horizontal_) {
+    1213             : 
+    1214         682 :       case STOP_MOTION_STATE: {
+    1215             : 
+    1216         682 :         stopHorizontalMotion();
+    1217         682 :         break;
+    1218             :       }
+    1219             : 
+    1220       20839 :       case STOPPING_STATE: {
+    1221             : 
+    1222       20839 :         stopHorizontal();
+    1223       20839 :         break;
+    1224             :       }
+    1225             : 
+    1226        1816 :       default: {
+    1227             : 
+    1228        1816 :         break;
+    1229             :       }
+    1230             :     }
+    1231             : 
+    1232       23337 :     switch (current_state_vertical_) {
+    1233             : 
+    1234         682 :       case STOP_MOTION_STATE: {
+    1235             : 
+    1236         682 :         stopVerticalMotion();
+    1237         682 :         break;
+    1238             :       }
+    1239             : 
+    1240       16042 :       case ACCELERATING_STATE: {
+    1241             : 
+    1242       16042 :         accelerateVertical();
+    1243       16042 :         break;
+    1244             :       }
+    1245             : 
+    1246        4797 :       case DECELERATING_STATE: {
+    1247             : 
+    1248        4797 :         decelerateVertical();
+    1249        4797 :         break;
+    1250             :       }
+    1251             : 
+    1252           0 :       case STOPPING_STATE: {
+    1253             : 
+    1254           0 :         stopVertical();
+    1255           0 :         break;
+    1256             :       }
+    1257             : 
+    1258        1816 :       default: {
+    1259             : 
+    1260        1816 :         break;
+    1261             :       }
+    1262             :     }
+    1263             :   }
+    1264             : 
+    1265       23912 :   if (current_state_horizontal_ == STOP_MOTION_STATE && current_state_vertical_ == STOP_MOTION_STATE) {
+    1266         702 :     if (fabs(current_vertical_speed) <= 0.1 && fabs(current_horizontal_speed) <= 0.1) {
+    1267             : 
+    1268             :       // if the current motion was stopped (the conditions above) but we still have a goal (landing or taking off)
+    1269             :       // -> we should start accelerating towards the goal in the vertical direction
+    1270             :       // This is important, do not modify without testing, otherwise your landing routine may crash into the ground
+    1271             :       // while having large lateral speed
+    1272          59 :       if (have_goal_) {
+    1273             : 
+    1274          34 :         changeStateVertical(ACCELERATING_STATE);
+    1275          34 :         changeStateHorizontal(STOPPING_STATE);
+    1276             : 
+    1277             :       } else {
+    1278             : 
+    1279          25 :         changeState(STOPPING_STATE);
+    1280             : 
+    1281             :         {
+    1282          25 :           std::scoped_lock lock(mutex_state_);
+    1283             : 
+    1284          25 :           current_horizontal_speed_ = 0;
+    1285          25 :           current_vertical_speed_   = 0;
+    1286             :         }
+    1287             :       }
+    1288             :     }
+    1289             :   }
+    1290             : 
+    1291       23912 :   if (current_state_vertical_ == STOPPING_STATE && current_state_horizontal_ == STOPPING_STATE) {
+    1292             : 
+    1293          46 :     if (fabs(state_x - goal_x) > 1.0 || fabs(state_y - goal_y) > 1.0 || fabs(state_z - goal_z) > 1.0) {
+    1294             : 
+    1295           0 :       ROS_ERROR("[LandoffTracker]: distance to the goal is too large when STOPPING, this could have been caused by a race condition!");
+    1296           0 :       ROS_ERROR("[LandoffTracker]: call for Tomas!!");
+    1297             : 
+    1298           0 :       cause_failsafe_ = true;
+    1299             : 
+    1300           0 :       changeState(HOVER_STATE);
+    1301             : 
+    1302          46 :     } else if (fabs(state_x - goal_x) < 0.1 && fabs(state_y - goal_y) < 0.1 && fabs(state_z - goal_z) < 0.1) {
+    1303             : 
+    1304             :       {
+    1305          46 :         std::scoped_lock lock(mutex_state_);
+    1306             : 
+    1307          46 :         if (!taking_off_) {
+    1308          25 :           state_x_ = goal_x;
+    1309          25 :           state_y_ = goal_y;
+    1310          25 :           state_z_ = goal_z;
+    1311             :         }
+    1312             : 
+    1313          46 :         current_horizontal_speed_ = 0;
+    1314          46 :         current_vertical_speed_   = 0;
+    1315             :       }
+    1316             : 
+    1317          46 :       changeState(HOVER_STATE);
+    1318             : 
+    1319          46 :       have_goal_ = false;
+    1320             :     }
+    1321             :   }
+    1322             : 
+    1323       23912 :   if (current_state_horizontal_ == LANDED_STATE && current_state_vertical_ == LANDED_STATE) {
+    1324             :     {
+    1325           0 :       std::scoped_lock lock(mutex_state_);
+    1326             : 
+    1327           0 :       state_x_ = goal_x = uav_x;
+    1328           0 :       state_y_ = goal_y = uav_y;
+    1329           0 :       state_z_ = goal_z = uav_z;
+    1330             : 
+    1331           0 :       have_goal_ = false;
+    1332             :     }
+    1333             :   }
+    1334             : 
+    1335             :   // --------------------------------------------------------------
+    1336             :   // |              motion saturation during takeoff              |
+    1337             :   // --------------------------------------------------------------
+    1338             : 
+    1339             :   // update the inner states
+    1340       23912 :   if (!takeoff_saturated) {
+    1341             :     {
+    1342       23337 :       std::scoped_lock lock(mutex_state_);
+    1343             : 
+    1344       23337 :       state_x_ += cos(current_heading) * current_horizontal_speed * _tracker_dt_;
+    1345       23337 :       state_y_ += sin(current_heading) * current_horizontal_speed * _tracker_dt_;
+    1346       23337 :       state_z_ += current_vertical_direction * current_vertical_speed * _tracker_dt_;
+    1347             :     }
+    1348             :   }
+    1349             : 
+    1350             :   // --------------------------------------------------------------
+    1351             :   // |                        heading tracking                        |
+    1352             :   // --------------------------------------------------------------
+    1353             : 
+    1354             :   // compute the desired heading rate
+    1355             :   {
+    1356       47824 :     std::scoped_lock lock(mutex_state_);
+    1357             : 
+    1358             :     double current_heading_rate;
+    1359             : 
+    1360       23912 :     if (fabs(goal_heading_ - state_heading_) > M_PI)
+    1361           0 :       current_heading_rate = -_heading_gain_ * (goal_heading_ - state_heading_);
+    1362             :     else
+    1363       23912 :       current_heading_rate = _heading_gain_ * (goal_heading_ - state_heading_);
+    1364             : 
+    1365       23912 :     if (current_heading_rate > _heading_rate_) {
+    1366           0 :       current_heading_rate = _heading_rate_;
+    1367       23912 :     } else if (current_heading_rate < -_heading_rate_) {
+    1368           0 :       current_heading_rate = -_heading_rate_;
+    1369             :     }
+    1370             : 
+    1371             :     // flap the resulted state_heading_ aroud PI
+    1372       23912 :     state_heading_ += current_heading_rate * _tracker_dt_;
+    1373             : 
+    1374       23912 :     if (state_heading_ > M_PI) {
+    1375           0 :       state_heading_ -= 2 * M_PI;
+    1376       23912 :     } else if (state_heading_ < -M_PI) {
+    1377           0 :       state_heading_ += 2 * M_PI;
+    1378             :     }
+    1379             : 
+    1380       23912 :     if (fabs(state_heading_ - goal_heading_) < (2 * (_heading_rate_ * _tracker_dt_))) {
+    1381       23912 :       state_heading_ = goal_heading_;
+    1382             :     }
+    1383             :   }
+    1384             : 
+    1385             :   // --------------------------------------------------------------
+    1386             :   // |                      landing setpoint                      |
+    1387             :   // --------------------------------------------------------------
+    1388             : 
+    1389       23912 :   if (landing_) {
+    1390             :     {
+    1391       11864 :       std::scoped_lock lock(mutex_goal_);
+    1392             : 
+    1393       11864 :       goal_z_ = uav_z + _landing_reference_;
+    1394             :     }
+    1395             :   }
+    1396             : }
+    1397             : 
+    1398             : //}
+    1399             : 
+    1400             : // | ------------------------ callbacks ----------------------- |
+    1401             : 
+    1402             : /* //{ callbackTakeoff() */
+    1403             : 
+    1404          21 : bool LandoffTracker::callbackTakeoff(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    1405             : 
+    1406          42 :   std::stringstream ss;
+    1407             : 
+    1408             :   // copy member variables
+    1409          42 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1410             : 
+    1411          21 :   double uav_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1412             : 
+    1413             :   double uav_x, uav_y, uav_z;
+    1414          21 :   uav_x = uav_state.pose.position.x;
+    1415          21 :   uav_y = uav_state.pose.position.y;
+    1416          21 :   uav_z = uav_state.pose.position.z;
+    1417             : 
+    1418          21 :   if (!is_active_) {
+    1419           0 :     ss << "can not takeoff, the tracker is not active";
+    1420           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[LandoffTracker]: " << ss.str());
+    1421           0 :     res.success = false;
+    1422           0 :     res.message = ss.str();
+    1423           0 :     return true;
+    1424             :   }
+    1425             : 
+    1426          21 :   if (!callbacks_enabled_) {
+    1427           0 :     ss << "can not takeoff, the callbacks are disabled";
+    1428           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[LandoffTracker]: " << ss.str());
+    1429           0 :     res.success = false;
+    1430           0 :     res.message = ss.str();
+    1431           0 :     return true;
+    1432             :   }
+    1433             : 
+    1434          21 :   if (req.goal < 0.5 || req.goal > 10.0) {
+    1435             : 
+    1436           0 :     ss << "can not takeoff, the goal should be within [0.5, 10.0] m!";
+    1437           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[LandoffTracker]: " << ss.str());
+    1438           0 :     res.success = false;
+    1439           0 :     res.message = ss.str();
+    1440           0 :     return true;
+    1441             :   }
+    1442             : 
+    1443             :   {
+    1444          42 :     std::scoped_lock lock(mutex_goal_, mutex_state_);
+    1445             : 
+    1446          21 :     state_x_ = uav_x;
+    1447          21 :     goal_x_  = uav_x;
+    1448             : 
+    1449          21 :     state_y_ = uav_y;
+    1450          21 :     goal_y_  = uav_y;
+    1451             : 
+    1452          21 :     state_z_ = uav_z;
+    1453          21 :     goal_z_  = uav_z + req.goal;
+    1454             : 
+    1455          21 :     state_heading_ = uav_heading;
+    1456          21 :     goal_heading_  = uav_heading;
+    1457             : 
+    1458          21 :     speed_x_                = 0;
+    1459          21 :     speed_y_                = 0;
+    1460          21 :     current_vertical_speed_ = 0;
+    1461             : 
+    1462          21 :     have_goal_ = true;
+    1463             :   }
+    1464             : 
+    1465          21 :   ROS_INFO("[LandoffTracker]: taking off");
+    1466             : 
+    1467          21 :   taking_off_ = true;
+    1468          21 :   landing_    = false;
+    1469          21 :   elanding_   = false;
+    1470             : 
+    1471          21 :   res.success = true;
+    1472          21 :   res.message = "taking off";
+    1473             : 
+    1474          21 :   changeState(STOP_MOTION_STATE);
+    1475             : 
+    1476          21 :   return true;
+    1477             : }
+    1478             : 
+    1479             : //}
+    1480             : 
+    1481             : /* //{ callbackLand() */
+    1482             : 
+    1483           5 : bool LandoffTracker::callbackLand([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1484             : 
+    1485          10 :   std::scoped_lock lock(mutex_main_timer_);
+    1486             : 
+    1487          10 :   std::stringstream ss;
+    1488             : 
+    1489             :   // copy member variables
+    1490          10 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1491             : 
+    1492           5 :   if (!is_active_) {
+    1493           0 :     ss << "can not land, the tracker is not active";
+    1494           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[LandoffTracker]: " << ss.str());
+    1495           0 :     res.success = false;
+    1496           0 :     res.message = ss.str();
+    1497           0 :     return true;
+    1498             :   }
+    1499             : 
+    1500             :   {
+    1501           5 :     std::scoped_lock lock(mutex_goal_);
+    1502             : 
+    1503           5 :     goal_z_ = uav_state.pose.position.z + _landing_reference_;
+    1504             :   }
+    1505             : 
+    1506           5 :   ROS_INFO("[LandoffTracker]: landing");
+    1507             : 
+    1508           5 :   landing_    = true;
+    1509           5 :   elanding_   = false;
+    1510           5 :   taking_off_ = false;
+    1511           5 :   have_goal_  = true;
+    1512             : 
+    1513           5 :   res.success = true;
+    1514           5 :   res.message = "landing";
+    1515             : 
+    1516           5 :   changeState(STOP_MOTION_STATE);
+    1517             : 
+    1518           5 :   return true;
+    1519             : }
+    1520             : 
+    1521             : //}
+    1522             : 
+    1523             : /* //{ callbackELand() */
+    1524             : 
+    1525           8 : bool LandoffTracker::callbackELand([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    1526             : 
+    1527          16 :   std::scoped_lock lock(mutex_main_timer_);
+    1528             : 
+    1529          16 :   std::stringstream ss;
+    1530             : 
+    1531             :   // copy member variables
+    1532          16 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1533             : 
+    1534           8 :   if (!is_active_) {
+    1535             : 
+    1536           0 :     ss << "can not eland, the tracker is not active";
+    1537           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[LandoffTracker]: " << ss.str());
+    1538           0 :     res.success = false;
+    1539           0 :     res.message = ss.str();
+    1540           0 :     taking_off_ = false;
+    1541           0 :     landing_    = false;
+    1542           0 :     elanding_   = false;
+    1543           0 :     changeState(LANDED_STATE);
+    1544           0 :     return true;
+    1545             :   }
+    1546             : 
+    1547             :   {
+    1548           8 :     std::scoped_lock lock(mutex_goal_);
+    1549             : 
+    1550           8 :     goal_z_ = uav_state.pose.position.z + _landing_reference_;
+    1551             :   }
+    1552             : 
+    1553           8 :   ROS_WARN("[LandoffTracker]: emergency landing");
+    1554             : 
+    1555           8 :   landing_    = true;
+    1556           8 :   elanding_   = true;
+    1557           8 :   taking_off_ = false;
+    1558           8 :   have_goal_  = true;
+    1559             : 
+    1560           8 :   res.success = true;
+    1561           8 :   res.message = "elanding";
+    1562             : 
+    1563           8 :   changeState(STOP_MOTION_STATE);
+    1564             : 
+    1565           8 :   return true;
+    1566             : }
+    1567             : 
+    1568             : //}
+    1569             : 
+    1570             : }  // namespace landoff_tracker
+    1571             : 
+    1572             : }  // namespace mrs_uav_trackers
+    1573             : 
+    1574             : #include <pluginlib/class_list_macros.h>
+    1575         109 : 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..62319f27de --- /dev/null +++ b/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.overview.html @@ -0,0 +1,414 @@ + + + + + + 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 + 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..8500838c4067e570cee460b14a6868e3e1e907d9 GIT binary patch literal 4860 zcmVRGM`lrZ`=Voca;I z#^6P?7tJwTcV+r!n4ZcbRt!{DJx@iQbRE;L?g2gH)TcCFy4Q@V&Qc#x^<*^C)ZJqS zarDZ-i)jJH{#@V0`X|;@v8fdsWbruB0{h8x?o00GErl`Tuh&L1r>TvmrRqsLvhZC^ z8>a~pJ!_aTP%50z7QlYw;4JV9*&=7gQ<_lqjB`b(dX^cAEU>}2FdWV|+R$l1>JysA z)lT{i@eW=Vfs0yuSc;B<=Vne+?ZC)O&H!cZw5W}9%)3N9*IN;Ak^)bp065z=w)Tu~ zoF%IvD6JnzZ_dw6apCERmgAx}PN9RH4+1!^ z;d;KwdU}x>tpF@l9p;_O^C<#fh&)hS@RXNn!ov72ZS{vPv@V%l;CN-KU702R#8AXkE56je-BjP$lc2U2U6F=7a4x>WLmP|nAOFX|QsZg`GehRBW`TRf;{1VCK@P)Jc+?4X$!Z{{aEzfX082FQ`BI{Lh} zXDFSZOd5)O(>4lxJibtQp8q|+j@Lg^9;O)J>#xTnUv7^Q!U5G?xP$=H45yH$F3c7M zP|?@M;@F>iOl;Nmb+RDp3gBPWe^q~6)mwjNwh`j#PjNuishRDPN&tSs^kK#Ga?bMp zS#8gFbG}i!N{X)++m?6vMgcGRlz_tWIc)@^L$F5ad+dZ zSalXyQ+2Lx!gUTPt-617X0;u{mas%s*ziGZgdz+wMeiJNUkQ-1=P6PkoqnphUhJd6VnQZ55h|+vDp}uVRniWGZ3Q>{8sPRWY~Zp;g3+OEne& z-=ny7aa@0}?0e$4(r<*F1V0uUy=H6fO$0f&6Sjs04H|$<)nkCc0;EQU!q~g2BQLHE z2pVa-s9_;amvO!V_jf#uDI%fireiX9T1T6fvH3|Oc;|A zrU5)|!XTF-$~8>j9q%`bEOWFKU@S>V&Ac&|SM7%a6Q+UV4vxa?zS#G80Q_(jSG~f_ z-OM1b@F}+ZZr7OA$7}+XPBBkYU>ARrJ!<_z_>G)B%-X35>5{d8>(LGpT=bEusqPWA z1_yZQXu3Hj%IUE>5pS{Af-dh0eFr^O=Vxt)8p5Ue>JbPy&uRM^6 z1va;_zmaM)_CQjB8DSgw_#?NQGQgU}$70MU8&Y9zzwp{Iq%PC#aU`5E>L9Uw%>)ES za-B^VNa!Osf8NdkUm%vJ_zfBkSHszZHXpSb?shpRc4?^rM_nis&&ts2wbHbNthvZm z?RAXldF=s_?T%-@*jxl4o&_y1xOTd96BCAxZx)-uH7_+|dyp#c26t|w*Nmz*_RG8qYx-!F?-TbGOZ)z9l=t=6)yGvg z8Gz=@G)I8H|s^PLInP|mcn{l2?lRzq(jw`5BFQE-(2U*qN%0@p_ z6y4wePOV*Vf~|>oaRyofw+}Qc_GM|h3^yu(8I!$&L?4nZ70e7WQppuP=ot^}893)= zHu~A8*q*SR-X)y-oP}&1q=V}=+=bG7TtyZ#t2R%cZu~U=Jn&iAQW=sn{3pjAazx>t<10BDG^5ZB z=zMqx^$ZY*8=40TEKroT5hc5WLOvUKn}oZC2rO;Qs#ZO{T)dp^Bq~l+{xJ#|{%}CM zUD-1ynHSAB0WCJGXc}WrZu)|c^{DRa^W;bQ9Qzoc!G86b3xiMbgukEu)lo+gESc(H`jpte{ zo{ORp^z+G3>H##p9s>uh3lHx^rRI-8o)j!8y8pLt=1co-h1`kb3j84HWsI@BY`z3obhP(%_2+lX||`6&=UfWgdWy2NA@d6(ix^g|3HEF zka&M%pED`YihPJkfe%-~_n|Ls&w%LW_$0+mvIA>puJmO{?4_XXslenGi3CtgaSar6 znUWfcvV%wvcu6L81FW`c5m10yRq0P_NrIt`vZ_}(GcsEt4~NOQjz^TsQ#&)(HfIYz z?8tJjHCM{^(8T0Q+e0s#NdYsar^WUpl)c9G?^R?2pb z19s>XD<1L10YYZhaAleg7UqC=dj7hp>WvxB5raMnF?Sx<#OzV78_Az>8T`iulm(F$ zJp&AB>hh-zGV_%_`=Maw>x!^(VgP$uT_) zgVDcs&XU6rXkY*qhT#bmX|7I=#bNm33AHgb3=cUoW@AyxOm`Qc`zc>HVNc-^OgzjA z=Va`Fa*Mc5i8}@kd5niY;F81Hu;O+9z@8X^y1|92rVj-{<%cXPGG=AWL2bHXr{(&) zPT)&8zBb!iSFW2|+((x-<4Yo7z2FA^mByMw1L;-kI{=<7i7xN=o%PWxP5P9;`juwN zAD}DEv@^Y61=XqwxNX4f45 z7t}^D{48{Wi$L#s6r4?Nk7Ho)XTI^rdw~@K8r{t{*HA(eRp6VuKM&Vvd znUVGk%=zecK!iySNjRD^uA3f8WPepwy@mz$x!&H=QsaMQ~@tk-Yr>otEO^?y@S} zMNoVB%f(9LL(#>Ds)V@5H{@ud6FzM@njLTPk8X1Z?oU&meLBTnO;H%{G535(lw!!H zohoC9-Mt3>QUJO?;EYrMDlB|<<_GwKqqw{OqZQ>2*vc|9||gQ;*+Giy}c>SHxe%1v%z4+Mg4+1>r={`b8@ zc34ZYI})T#Zs6qzHhp(Gv_pG(0RvXa%xu#$uAOc0z-;$SyHi_Mnidah1;15Q-~0~y z;`ShCaFM9l9K3->h9~WT>-M0)(S|{XQ3xms9`H<~6i#8!ZZWUjth8XC1MJ1v-zQ4y zOpMzc_WP|fyGfMPiM>8L1J|TTn)~(d`gp%XVD=)7_eO@dltik^J9Edm1t>d*aSOLSem>z|wM+@i z1D}LzABs5Hv=H-@?vP06(*@mwDCUq|=(8Rt@8o(Pe1bqgkx7v>;_+;k?|zVReg3#J zMG9=F`qn*VC~jkhYiS#0O1{FnGieRFo{?Pj69LbMi6(&(3m|5C08U)^9PQ|IzUJA0 zIC1y&!`U4CUXck%H$Se^MAsvF%+Kou_w0i6^CRAKs!8zc_1G=a#w1C_<3{mwTg{K% z8#w=7XHsO^vku9W_F=OEaP?aC?Lesk5RSrCb{$!~V$T4_CX)AAN_o3bScw0Kz!0#D z@x=zZa-#qu{}E;?4L$vD8e7pDN)zP`7buKOXp03Fkw5 zmp4%q5lx}uuA>r8H+BofOtn;j-gs5?MR6xNJ1uQ|=MMYWv_^Nb8+_1q z^<4NVX1uFXveCv7O#ThFPyo0og7ZiTsRV=+3|@C1>cW<$0`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:32547768.1 %
Date:2024-12-17 22:35:29Functions: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 +
68.1%68.1%
+ 		68.1 %325 / 47763.3 %19 / 30
<unnamed>68.1 %325 / 47763.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..9da7d59cd4 --- /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:32547768.1 %
Date:2024-12-17 22:35:29Functions: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 +
68.1%68.1%
+ 		68.1 %325 / 47763.3 %19 / 30
<unnamed>68.1 %325 / 47763.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..f5d1d50650 --- /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:32547768.1 %
Date:2024-12-17 22:35:29Functions: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 +
68.1%68.1%
+ 		68.1 %325 / 47763.3 %19 / 30
<unnamed>68.1 %325 / 47763.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..9c21f2aefe --- /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:32547768.1 %
Date:2024-12-17 22:35:29Functions: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 +
68.1%68.1%
+ 		68.1 %325 / 47763.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..666d82214e --- /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:32547768.1 %
Date:2024-12-17 22:35:29Functions: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 +
68.1%68.1%
+ 		68.1 %325 / 47763.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..9d1bfcc71d --- /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:32547768.1 %
Date:2024-12-17 22:35:29Functions: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 +
68.1%68.1%
+ 		68.1 %325 / 47763.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..2ff9fc8de9 --- /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:32547768.1 %
Date:2024-12-17 22:35:29Functions: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::stopVerticalMotion()2
mrs_uav_trackers::line_tracker::LineTracker::stopHorizontalMotion()2
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)6
mrs_uav_trackers::line_tracker::LineTracker::changeStateVertical(mrs_uav_trackers::line_tracker::States_t)8
mrs_uav_trackers::line_tracker::LineTracker::changeStateHorizontal(mrs_uav_trackers::line_tracker::States_t)8
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()179
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&)1702
mrs_uav_trackers::line_tracker::LineTracker::mainTimer(ros::TimerEvent const&)2213
+
+
+ + + +
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..4621b8e1dc --- /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:32547768.1 %
Date:2024-12-17 22:35:29Functions: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)6
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()2
mrs_uav_trackers::line_tracker::LineTracker::changeStateVertical(mrs_uav_trackers::line_tracker::States_t)8
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()2
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)8
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&)1702
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()179
mrs_uav_trackers::line_tracker::LineTracker::mainTimer(ros::TimerEvent const&)2213
+
+
+ + + +
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..25446f67ce --- /dev/null +++ b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.html @@ -0,0 +1,1364 @@ + + + + + + + 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:32547768.1 %
Date:2024-12-17 22:35:29Functions: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        1702 : 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        5106 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     519        5106 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("LineTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     520             : 
+     521             :   {
+     522        1702 :     std::scoped_lock lock(mutex_uav_state_);
+     523             : 
+     524        1702 :     uav_state_ = uav_state;
+     525        1702 :     uav_x_     = uav_state_.pose.position.x;
+     526        1702 :     uav_y_     = uav_state_.pose.position.y;
+     527        1702 :     uav_z_     = uav_state_.pose.position.z;
+     528             : 
+     529        1702 :     got_uav_state_ = true;
+     530             :   }
+     531             : 
+     532             :   // up to this part the update() method is evaluated even when the tracker is not active
+     533        1702 :   if (!is_active_) {
+     534         106 :     return {};
+     535             :   }
+     536             : 
+     537        3192 :   mrs_msgs::TrackerCommand tracker_cmd;
+     538             : 
+     539        1596 :   tracker_cmd.header.stamp    = ros::Time::now();
+     540        1596 :   tracker_cmd.header.frame_id = uav_state.header.frame_id;
+     541             : 
+     542             :   {
+     543        1596 :     std::scoped_lock lock(mutex_state_);
+     544             : 
+     545        1596 :     tracker_cmd.position.x = state_x_;
+     546        1596 :     tracker_cmd.position.y = state_y_;
+     547        1596 :     tracker_cmd.position.z = state_z_;
+     548        1596 :     tracker_cmd.heading    = radians::wrap(state_heading_);
+     549             : 
+     550        1596 :     tracker_cmd.velocity.x   = cos(current_heading_) * current_horizontal_speed_;
+     551        1596 :     tracker_cmd.velocity.y   = sin(current_heading_) * current_horizontal_speed_;
+     552        1596 :     tracker_cmd.velocity.z   = current_vertical_direction_ * current_vertical_speed_;
+     553        1596 :     tracker_cmd.heading_rate = speed_heading_;
+     554             : 
+     555        1596 :     tracker_cmd.acceleration.x = 0;
+     556        1596 :     tracker_cmd.acceleration.y = 0;
+     557        1596 :     tracker_cmd.acceleration.z = current_vertical_direction_ * current_vertical_acceleration_;
+     558             : 
+     559        1596 :     tracker_cmd.use_position_vertical   = 1;
+     560        1596 :     tracker_cmd.use_position_horizontal = 1;
+     561        1596 :     tracker_cmd.use_heading             = 1;
+     562        1596 :     tracker_cmd.use_heading_rate        = 1;
+     563        1596 :     tracker_cmd.use_velocity_vertical   = 1;
+     564        1596 :     tracker_cmd.use_velocity_horizontal = 1;
+     565        1596 :     tracker_cmd.use_acceleration        = 1;
+     566             :   }
+     567             : 
+     568        1596 :   return {tracker_cmd};
+     569             : }
+     570             : 
+     571             : //}
+     572             : 
+     573             : /* //{ getStatus() */
+     574             : 
+     575         179 : const mrs_msgs::TrackerStatus LineTracker::getStatus() {
+     576             : 
+     577         179 :   mrs_msgs::TrackerStatus tracker_status;
+     578             : 
+     579         179 :   tracker_status.active            = is_active_;
+     580         179 :   tracker_status.callbacks_enabled = callbacks_enabled_;
+     581             : 
+     582         179 :   const bool idling = current_state_vertical_ == IDLE_STATE && current_state_horizontal_ == IDLE_STATE;
+     583             : 
+     584         179 :   if (idling)
+     585          64 :     tracker_status.state = mrs_msgs::TrackerStatus::STATE_IDLE;
+     586             :   else
+     587         115 :     tracker_status.state = mrs_msgs::TrackerStatus::STATE_REFERENCE;
+     588             : 
+     589         179 :   tracker_status.have_goal = !idling;
+     590             : 
+     591         179 :   tracker_status.tracking_trajectory = false;
+     592             : 
+     593         179 :   return tracker_status;
+     594             : }
+     595             : 
+     596             : //}
+     597             : 
+     598             : /* //{ enableCallbacks() */
+     599             : 
+     600           0 : const std_srvs::SetBoolResponse::ConstPtr LineTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     601             : 
+     602           0 :   std_srvs::SetBoolResponse res;
+     603           0 :   std::stringstream         ss;
+     604             : 
+     605           0 :   if (cmd->data != callbacks_enabled_) {
+     606             : 
+     607           0 :     callbacks_enabled_ = cmd->data;
+     608             : 
+     609           0 :     ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+     610           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[LineTracker]: " << ss.str());
+     611             : 
+     612             :   } else {
+     613             : 
+     614           0 :     ss << "callbacks were already " << (callbacks_enabled_ ? "enabled" : "disabled");
+     615           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[LineTracker]: " << ss.str());
+     616             :   }
+     617             : 
+     618           0 :   res.message = ss.str();
+     619           0 :   res.success = true;
+     620             : 
+     621           0 :   return std_srvs::SetBoolResponse::ConstPtr(new std_srvs::SetBoolResponse(res));
+     622             : }
+     623             : 
+     624             : //}
+     625             : 
+     626             : /* switchOdometrySource() //{ */
+     627             : 
+     628           0 : const std_srvs::TriggerResponse::ConstPtr LineTracker::switchOdometrySource(const mrs_msgs::UavState &new_uav_state) {
+     629             : 
+     630           0 :   std::scoped_lock lock(mutex_goal_, mutex_state_);
+     631             : 
+     632           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     633             : 
+     634           0 :   double old_heading  = 0;
+     635           0 :   double new_heading  = 0;
+     636           0 :   bool   got_headings = true;
+     637             : 
+     638             :   try {
+     639           0 :     old_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     640             :   }
+     641           0 :   catch (...) {
+     642           0 :     ROS_ERROR_THROTTLE(1.0, "[LineTracker]: could not calculate the old UAV heading");
+     643           0 :     got_headings = false;
+     644             :   }
+     645             : 
+     646             :   try {
+     647           0 :     new_heading = mrs_lib::AttitudeConverter(new_uav_state.pose.orientation).getHeading();
+     648             :   }
+     649           0 :   catch (...) {
+     650           0 :     ROS_ERROR_THROTTLE(1.0, "[LineTracker]: could not calculate the new UAV heading");
+     651           0 :     got_headings = false;
+     652             :   }
+     653             : 
+     654           0 :   std_srvs::TriggerResponse res;
+     655             : 
+     656           0 :   if (!got_headings) {
+     657           0 :     res.message = "could not calculate the heading difference";
+     658           0 :     res.success = false;
+     659             : 
+     660           0 :     return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+     661             :   }
+     662             : 
+     663             :   // | --------- recalculate the goal to new coordinates -------- |
+     664             : 
+     665           0 :   double dx       = new_uav_state.pose.position.x - uav_state.pose.position.x;
+     666           0 :   double dy       = new_uav_state.pose.position.y - uav_state.pose.position.y;
+     667           0 :   double dz       = new_uav_state.pose.position.z - uav_state.pose.position.z;
+     668           0 :   double dheading = new_heading - old_heading;
+     669             : 
+     670           0 :   goal_x_ += dx;
+     671           0 :   goal_y_ += dy;
+     672           0 :   goal_z_ += dz;
+     673           0 :   goal_heading_ += dheading;
+     674             : 
+     675             :   // | -------------------- update the state -------------------- |
+     676             : 
+     677           0 :   state_x_ += dx;
+     678           0 :   state_y_ += dy;
+     679           0 :   state_z_ += dz;
+     680           0 :   state_heading_ += dheading;
+     681             : 
+     682           0 :   current_heading_ = atan2(goal_y_ - state_y_, goal_x_ - state_x_);
+     683             : 
+     684           0 :   res.message = "odometry source switched";
+     685           0 :   res.success = true;
+     686             : 
+     687           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+     688             : }
+     689             : 
+     690             : //}
+     691             : 
+     692             : /* //{ hover() */
+     693             : 
+     694           0 : const std_srvs::TriggerResponse::ConstPtr LineTracker::hover([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     695             : 
+     696           0 :   std_srvs::TriggerResponse res;
+     697             : 
+     698             :   // --------------------------------------------------------------
+     699             :   // |          horizontal initial conditions prediction          |
+     700             :   // --------------------------------------------------------------
+     701             :   {
+     702           0 :     std::scoped_lock lock(mutex_state_, mutex_uav_state_);
+     703             : 
+     704           0 :     current_horizontal_speed_ = sqrt(pow(uav_state_.velocity.linear.x, 2) + pow(uav_state_.velocity.linear.y, 2));
+     705           0 :     current_vertical_speed_   = uav_state_.velocity.linear.z;
+     706           0 :     current_heading_          = atan2(uav_state_.velocity.linear.y, uav_state_.velocity.linear.x);
+     707             :   }
+     708             : 
+     709             :   double horizontal_t_stop, horizontal_stop_dist, stop_dist_x, stop_dist_y;
+     710             : 
+     711             :   {
+     712           0 :     std::scoped_lock lock(mutex_state_);
+     713             : 
+     714           0 :     horizontal_t_stop    = current_horizontal_speed_ / _horizontal_acceleration_;
+     715           0 :     horizontal_stop_dist = (horizontal_t_stop * current_horizontal_speed_) / 2.0;
+     716           0 :     stop_dist_x          = cos(current_heading_) * horizontal_stop_dist;
+     717           0 :     stop_dist_y          = sin(current_heading_) * horizontal_stop_dist;
+     718             :   }
+     719             : 
+     720             :   // --------------------------------------------------------------
+     721             :   // |           vertical initial conditions prediction           |
+     722             :   // --------------------------------------------------------------
+     723             : 
+     724             :   double vertical_t_stop, vertical_stop_dist;
+     725             : 
+     726             :   {
+     727           0 :     std::scoped_lock lock(mutex_state_);
+     728             : 
+     729           0 :     vertical_t_stop    = current_vertical_speed_ / _vertical_acceleration_;
+     730           0 :     vertical_stop_dist = current_vertical_direction_ * (vertical_t_stop * current_vertical_speed_) / 2.0;
+     731             :   }
+     732             : 
+     733             :   // --------------------------------------------------------------
+     734             :   // |                        set the goal                        |
+     735             :   // --------------------------------------------------------------
+     736             : 
+     737             :   {
+     738           0 :     std::scoped_lock lock(mutex_goal_, mutex_state_);
+     739             : 
+     740           0 :     goal_x_ = state_x_ + stop_dist_x;
+     741           0 :     goal_y_ = state_y_ + stop_dist_y;
+     742           0 :     goal_z_ = state_z_ + vertical_stop_dist;
+     743             :   }
+     744             : 
+     745           0 :   res.message = "hover initiated";
+     746           0 :   res.success = true;
+     747             : 
+     748           0 :   changeState(STOP_MOTION_STATE);
+     749             : 
+     750           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+     751             : }
+     752             : 
+     753             : //}
+     754             : 
+     755             : /* //{ startTrajectoryTracking() */
+     756             : 
+     757           0 : const std_srvs::TriggerResponse::ConstPtr LineTracker::startTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     758           0 :   return std_srvs::TriggerResponse::Ptr();
+     759             : }
+     760             : 
+     761             : //}
+     762             : 
+     763             : /* //{ stopTrajectoryTracking() */
+     764             : 
+     765           0 : const std_srvs::TriggerResponse::ConstPtr LineTracker::stopTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     766           0 :   return std_srvs::TriggerResponse::Ptr();
+     767             : }
+     768             : 
+     769             : //}
+     770             : 
+     771             : /* //{ resumeTrajectoryTracking() */
+     772             : 
+     773           0 : const std_srvs::TriggerResponse::ConstPtr LineTracker::resumeTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     774           0 :   return std_srvs::TriggerResponse::Ptr();
+     775             : }
+     776             : 
+     777             : //}
+     778             : 
+     779             : /* //{ gotoTrajectoryStart() */
+     780             : 
+     781           0 : const std_srvs::TriggerResponse::ConstPtr LineTracker::gotoTrajectoryStart([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     782           0 :   return std_srvs::TriggerResponse::Ptr();
+     783             : }
+     784             : 
+     785             : //}
+     786             : 
+     787             : /* //{ setConstraints() */
+     788             : 
+     789           3 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr LineTracker::setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     790             : 
+     791           6 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     792             : 
+     793             :   // this is the place to copy the constraints
+     794             :   {
+     795           3 :     std::scoped_lock lock(mutex_constraints_);
+     796             : 
+     797           3 :     _horizontal_speed_        = cmd->constraints.horizontal_speed;
+     798           3 :     _horizontal_acceleration_ = cmd->constraints.horizontal_acceleration;
+     799             : 
+     800           3 :     _vertical_speed_        = cmd->constraints.vertical_ascending_speed;
+     801           3 :     _vertical_acceleration_ = cmd->constraints.vertical_ascending_acceleration;
+     802             : 
+     803           3 :     _heading_rate_ = cmd->constraints.heading_speed;
+     804             :   }
+     805             : 
+     806           3 :   res.success = true;
+     807           3 :   res.message = "constraints updated";
+     808             : 
+     809           6 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     810             : }
+     811             : 
+     812             : //}
+     813             : 
+     814             : /* //{ setReference() */
+     815             : 
+     816           1 : const mrs_msgs::ReferenceSrvResponse::ConstPtr LineTracker::setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr &cmd) {
+     817             : 
+     818           2 :   mrs_msgs::ReferenceSrvResponse res;
+     819             : 
+     820           1 :   auto state_heading = mrs_lib::get_mutexed(mutex_state_, state_heading_);
+     821             : 
+     822             :   {
+     823           1 :     std::scoped_lock lock(mutex_goal_);
+     824             : 
+     825           1 :     goal_x_       = cmd->reference.position.x;
+     826           1 :     goal_y_       = cmd->reference.position.y;
+     827           1 :     goal_z_       = cmd->reference.position.z;
+     828           1 :     goal_heading_ = radians::unwrap(cmd->reference.heading, state_heading);
+     829             : 
+     830           1 :     ROS_INFO("[LineTracker]: received new setpoint %.2f, %.2f, %.2f, %.2f", goal_x_, goal_y_, goal_z_, goal_heading_);
+     831             : 
+     832           1 :     have_goal_ = true;
+     833             :   }
+     834             : 
+     835           1 :   changeState(STOP_MOTION_STATE);
+     836             : 
+     837           1 :   res.success = true;
+     838           1 :   res.message = "reference set";
+     839             : 
+     840           2 :   return mrs_msgs::ReferenceSrvResponse::ConstPtr(new mrs_msgs::ReferenceSrvResponse(res));
+     841             : }
+     842             : 
+     843             : //}
+     844             : 
+     845             : /* //{ setVelocityReference() */
+     846             : 
+     847           0 : const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr LineTracker::setVelocityReference([
+     848             :     [maybe_unused]] const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr &cmd) {
+     849           0 :   return mrs_msgs::VelocityReferenceSrvResponse::Ptr();
+     850             : }
+     851             : 
+     852             : //}
+     853             : 
+     854             : /* //{ setTrajectoryReference() */
+     855             : 
+     856           0 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr LineTracker::setTrajectoryReference([
+     857             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     858           0 :   return mrs_msgs::TrajectoryReferenceSrvResponse::Ptr();
+     859             : }
+     860             : 
+     861             : //}
+     862             : 
+     863             : // | ----------------- state machine routines ----------------- |
+     864             : 
+     865             : /* //{ changeStateHorizontal() */
+     866             : 
+     867           8 : void LineTracker::changeStateHorizontal(States_t new_state) {
+     868             : 
+     869           8 :   previous_state_horizontal_ = current_state_horizontal_;
+     870           8 :   current_state_horizontal_  = new_state;
+     871             : 
+     872             :   // just for ROS_INFO
+     873           8 :   ROS_DEBUG("[LineTracker]: Switching horizontal state %s -> %s", state_names[previous_state_horizontal_], state_names[current_state_horizontal_]);
+     874           8 : }
+     875             : 
+     876             : //}
+     877             : 
+     878             : /* //{ changeStateVertical() */
+     879             : 
+     880           8 : void LineTracker::changeStateVertical(States_t new_state) {
+     881             : 
+     882           8 :   previous_state_vertical_ = current_state_vertical_;
+     883           8 :   current_state_vertical_  = new_state;
+     884             : 
+     885             :   // just for ROS_INFO
+     886           8 :   ROS_DEBUG("[LineTracker]: Switching vertical state %s -> %s", state_names[previous_state_vertical_], state_names[current_state_vertical_]);
+     887           8 : }
+     888             : 
+     889             : //}
+     890             : 
+     891             : /* //{ changeState() */
+     892             : 
+     893           6 : void LineTracker::changeState(States_t new_state) {
+     894             : 
+     895           6 :   changeStateVertical(new_state);
+     896           6 :   changeStateHorizontal(new_state);
+     897           6 : }
+     898             : 
+     899             : //}
+     900             : 
+     901             : // | --------------------- motion routines -------------------- |
+     902             : 
+     903             : /* //{ stopHorizontalMotion() */
+     904             : 
+     905           2 : void LineTracker::stopHorizontalMotion(void) {
+     906             : 
+     907             :   {
+     908           4 :     std::scoped_lock lock(mutex_state_);
+     909             : 
+     910           2 :     current_horizontal_speed_ -= _horizontal_acceleration_ * _tracker_dt_;
+     911             : 
+     912           2 :     if (current_horizontal_speed_ < 0) {
+     913           2 :       current_horizontal_speed_        = 0;
+     914           2 :       current_horizontal_acceleration_ = 0;
+     915             :     } else {
+     916           0 :       current_horizontal_acceleration_ = -_horizontal_acceleration_;
+     917             :     }
+     918             :   }
+     919           2 : }
+     920             : 
+     921             : //}
+     922             : 
+     923             : /* //{ stopVerticalMotion() */
+     924             : 
+     925           2 : void LineTracker::stopVerticalMotion(void) {
+     926             : 
+     927             :   {
+     928           4 :     std::scoped_lock lock(mutex_state_);
+     929             : 
+     930           2 :     current_vertical_speed_ -= _vertical_acceleration_ * _tracker_dt_;
+     931             : 
+     932           2 :     if (current_vertical_speed_ < 0) {
+     933           2 :       current_vertical_speed_        = 0;
+     934           2 :       current_vertical_acceleration_ = 0;
+     935             :     } else {
+     936           0 :       current_vertical_acceleration_ = -_vertical_acceleration_;
+     937             :     }
+     938             :   }
+     939           2 : }
+     940             : 
+     941             : //}
+     942             : 
+     943             : /* //{ accelerateHorizontal() */
+     944             : 
+     945        1004 : void LineTracker::accelerateHorizontal(void) {
+     946             : 
+     947             :   // copy member variables
+     948        1004 :   auto [goal_x, goal_y]                             = mrs_lib::get_mutexed(mutex_goal_, goal_x_, goal_y_);
+     949        1004 :   auto [state_x, state_y, current_horizontal_speed] = mrs_lib::get_mutexed(mutex_state_, state_x_, state_y_, current_horizontal_speed_);
+     950             : 
+     951             :   {
+     952        1004 :     std::scoped_lock lock(mutex_state_);
+     953             : 
+     954        1004 :     current_heading_ = atan2(goal_y - state_y, goal_x - state_x);
+     955             :   }
+     956             : 
+     957        1004 :   auto current_heading = mrs_lib::get_mutexed(mutex_state_, current_heading_);
+     958             : 
+     959             :   double horizontal_t_stop, horizontal_stop_dist, stop_dist_x, stop_dist_y;
+     960             : 
+     961        1004 :   horizontal_t_stop    = current_horizontal_speed / _horizontal_acceleration_;
+     962        1004 :   horizontal_stop_dist = (horizontal_t_stop * current_horizontal_speed) / 2.0;
+     963        1004 :   stop_dist_x          = cos(current_heading) * horizontal_stop_dist;
+     964        1004 :   stop_dist_y          = sin(current_heading) * horizontal_stop_dist;
+     965             : 
+     966             :   {
+     967        2008 :     std::scoped_lock lock(mutex_state_);
+     968             : 
+     969        1004 :     current_horizontal_speed_ += _horizontal_acceleration_ * _tracker_dt_;
+     970             : 
+     971        1004 :     if (current_horizontal_speed_ >= _horizontal_speed_) {
+     972         905 :       current_horizontal_speed_        = _horizontal_speed_;
+     973         905 :       current_horizontal_acceleration_ = 0;
+     974             :     } else {
+     975          99 :       current_horizontal_acceleration_ = _horizontal_acceleration_;
+     976             :     }
+     977             :   }
+     978             : 
+     979        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_))) {
+     980             : 
+     981             :     {
+     982           1 :       std::scoped_lock lock(mutex_state_);
+     983             : 
+     984           1 :       current_horizontal_acceleration_ = 0;
+     985             :     }
+     986             : 
+     987           1 :     changeStateHorizontal(DECELERATING_STATE);
+     988             :   }
+     989        1004 : }
+     990             : 
+     991             : //}
+     992             : 
+     993             : /* //{ accelerateVertical() */
+     994             : 
+     995         199 : void LineTracker::accelerateVertical(void) {
+     996             : 
+     997         199 :   auto goal_z                            = mrs_lib::get_mutexed(mutex_goal_, goal_z_);
+     998         199 :   auto [state_z, current_vertical_speed] = mrs_lib::get_mutexed(mutex_state_, state_z_, current_vertical_speed_);
+     999             : 
+    1000             :   // set the right heading
+    1001         199 :   double tar_z = goal_z - state_z;
+    1002             : 
+    1003             :   // set the right vertical direction
+    1004             :   {
+    1005         199 :     std::scoped_lock lock(mutex_state_);
+    1006             : 
+    1007         199 :     current_vertical_direction_ = mrs_lib::signum(tar_z);
+    1008             :   }
+    1009             : 
+    1010         199 :   auto current_vertical_direction = mrs_lib::get_mutexed(mutex_state_, current_vertical_direction_);
+    1011             : 
+    1012             :   // calculate the time to stop and the distance it will take to stop [vertical]
+    1013         199 :   double vertical_t_stop    = current_vertical_speed / _vertical_acceleration_;
+    1014         199 :   double vertical_stop_dist = (vertical_t_stop * current_vertical_speed) / 2.0;
+    1015         199 :   double stop_dist_z        = current_vertical_direction * vertical_stop_dist;
+    1016             : 
+    1017             :   {
+    1018         398 :     std::scoped_lock lock(mutex_state_);
+    1019             : 
+    1020         199 :     current_vertical_speed_ += _vertical_acceleration_ * _tracker_dt_;
+    1021             : 
+    1022         199 :     if (current_vertical_speed_ >= _vertical_speed_) {
+    1023         100 :       current_vertical_speed_        = _vertical_speed_;
+    1024         100 :       current_vertical_acceleration_ = 0;
+    1025             :     } else {
+    1026          99 :       current_vertical_acceleration_ = _vertical_acceleration_;
+    1027             :     }
+    1028             :   }
+    1029             : 
+    1030         199 :   if (fabs(state_z + stop_dist_z - goal_z) < (2 * (_vertical_speed_ * _tracker_dt_))) {
+    1031             : 
+    1032             :     {
+    1033           1 :       std::scoped_lock lock(mutex_state_);
+    1034             : 
+    1035           1 :       current_vertical_acceleration_ = 0;
+    1036             :     }
+    1037             : 
+    1038           1 :     changeStateVertical(DECELERATING_STATE);
+    1039             :   }
+    1040         199 : }
+    1041             : 
+    1042             : //}
+    1043             : 
+    1044             : /* //{ decelerateHorizontal() */
+    1045             : 
+    1046         100 : void LineTracker::decelerateHorizontal(void) {
+    1047             : 
+    1048             :   {
+    1049         200 :     std::scoped_lock lock(mutex_state_);
+    1050             : 
+    1051         100 :     current_horizontal_speed_ -= _horizontal_acceleration_ * _tracker_dt_;
+    1052             : 
+    1053         100 :     if (current_horizontal_speed_ < 0) {
+    1054           1 :       current_horizontal_speed_ = 0;
+    1055             :     } else {
+    1056          99 :       current_horizontal_acceleration_ = -_horizontal_acceleration_;
+    1057             :     }
+    1058             :   }
+    1059             : 
+    1060         100 :   auto current_horizontal_speed = mrs_lib::get_mutexed(mutex_state_, current_horizontal_speed_);
+    1061             : 
+    1062         100 :   if (current_horizontal_speed == 0) {
+    1063             : 
+    1064             :     {
+    1065           1 :       std::scoped_lock lock(mutex_state_);
+    1066             : 
+    1067           1 :       current_horizontal_acceleration_ = 0;
+    1068             :     }
+    1069             : 
+    1070           1 :     changeStateHorizontal(STOPPING_STATE);
+    1071             :   }
+    1072         100 : }
+    1073             : 
+    1074             : //}
+    1075             : 
+    1076             : /* //{ decelerateVertical() */
+    1077             : 
+    1078         100 : void LineTracker::decelerateVertical(void) {
+    1079             : 
+    1080             :   {
+    1081         200 :     std::scoped_lock lock(mutex_state_);
+    1082             : 
+    1083         100 :     current_vertical_speed_ -= _vertical_acceleration_ * _tracker_dt_;
+    1084             : 
+    1085         100 :     if (current_vertical_speed_ < 0) {
+    1086           1 :       current_vertical_speed_ = 0;
+    1087             :     } else {
+    1088          99 :       current_vertical_acceleration_ = -_vertical_acceleration_;
+    1089             :     }
+    1090             :   }
+    1091             : 
+    1092         100 :   auto current_vertical_speed = mrs_lib::get_mutexed(mutex_state_, current_vertical_speed_);
+    1093             : 
+    1094         100 :   if (current_vertical_speed == 0) {
+    1095           1 :     current_vertical_acceleration_ = 0;
+    1096           1 :     changeStateVertical(STOPPING_STATE);
+    1097             :   }
+    1098         100 : }
+    1099             : 
+    1100             : //}
+    1101             : 
+    1102             : /* //{ stopHorizontal() */
+    1103             : 
+    1104          46 : void LineTracker::stopHorizontal(void) {
+    1105             : 
+    1106             :   {
+    1107          46 :     std::scoped_lock lock(mutex_state_);
+    1108             : 
+    1109          46 :     state_x_                         = 0.95 * state_x_ + 0.05 * goal_x_;
+    1110          46 :     state_y_                         = 0.95 * state_y_ + 0.05 * goal_y_;
+    1111          46 :     current_horizontal_acceleration_ = 0;
+    1112             :   }
+    1113          46 : }
+    1114             : 
+    1115             : //}
+    1116             : 
+    1117             : /* //{ stopVertical() */
+    1118             : 
+    1119         851 : void LineTracker::stopVertical(void) {
+    1120             : 
+    1121             :   {
+    1122         851 :     std::scoped_lock lock(mutex_state_);
+    1123             : 
+    1124         851 :     state_z_                       = 0.95 * state_z_ + 0.05 * goal_z_;
+    1125         851 :     current_vertical_acceleration_ = 0;
+    1126             :   }
+    1127         851 : }
+    1128             : 
+    1129             : //}
+    1130             : 
+    1131             : // | ------------------------- timers ------------------------- |
+    1132             : 
+    1133             : /* //{ mainTimer() */
+    1134             : 
+    1135        2213 : void LineTracker::mainTimer(const ros::TimerEvent &event) {
+    1136             : 
+    1137        2213 :   if (!is_active_) {
+    1138         422 :     return;
+    1139             :   }
+    1140             : 
+    1141        5373 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("main", _tracker_loop_rate_, 0.01, event);
+    1142        5373 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("LineTracker::main", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1143             : 
+    1144        1791 :   auto [goal_x, goal_y, goal_z]    = mrs_lib::get_mutexed(mutex_goal_, goal_x_, goal_y_, goal_z_);
+    1145        1791 :   auto [state_x, state_y, state_z] = mrs_lib::get_mutexed(mutex_state_, state_x_, state_y_, state_z_);
+    1146             : 
+    1147        1791 :   switch (current_state_horizontal_) {
+    1148             : 
+    1149         639 :     case IDLE_STATE:
+    1150             : 
+    1151         639 :       break;
+    1152             : 
+    1153           2 :     case STOP_MOTION_STATE:
+    1154             : 
+    1155           2 :       stopHorizontalMotion();
+    1156             : 
+    1157           2 :       break;
+    1158             : 
+    1159        1004 :     case ACCELERATING_STATE:
+    1160             : 
+    1161        1004 :       accelerateHorizontal();
+    1162             : 
+    1163        1004 :       break;
+    1164             : 
+    1165         100 :     case DECELERATING_STATE:
+    1166             : 
+    1167         100 :       decelerateHorizontal();
+    1168             : 
+    1169         100 :       break;
+    1170             : 
+    1171          46 :     case STOPPING_STATE:
+    1172             : 
+    1173          46 :       stopHorizontal();
+    1174             : 
+    1175          46 :       break;
+    1176             :   }
+    1177             : 
+    1178        1791 :   switch (current_state_vertical_) {
+    1179             : 
+    1180         639 :     case IDLE_STATE:
+    1181             : 
+    1182         639 :       break;
+    1183             : 
+    1184           2 :     case STOP_MOTION_STATE:
+    1185             : 
+    1186           2 :       stopVerticalMotion();
+    1187             : 
+    1188           2 :       break;
+    1189             : 
+    1190         199 :     case ACCELERATING_STATE:
+    1191             : 
+    1192         199 :       accelerateVertical();
+    1193             : 
+    1194         199 :       break;
+    1195             : 
+    1196         100 :     case DECELERATING_STATE:
+    1197             : 
+    1198         100 :       decelerateVertical();
+    1199             : 
+    1200         100 :       break;
+    1201             : 
+    1202         851 :     case STOPPING_STATE:
+    1203             : 
+    1204         851 :       stopVertical();
+    1205             : 
+    1206         851 :       break;
+    1207             :   }
+    1208             : 
+    1209        1791 :   if (current_state_horizontal_ == STOP_MOTION_STATE && current_state_vertical_ == STOP_MOTION_STATE) {
+    1210           2 :     if (current_vertical_speed_ == 0 && current_horizontal_speed_ == 0) {
+    1211           2 :       if (have_goal_) {
+    1212           1 :         changeState(ACCELERATING_STATE);
+    1213             :       } else {
+    1214           1 :         changeState(STOPPING_STATE);
+    1215             :       }
+    1216             :     }
+    1217             :   }
+    1218             : 
+    1219        1791 :   if (current_state_horizontal_ == STOPPING_STATE && current_state_vertical_ == STOPPING_STATE) {
+    1220          48 :     if (fabs(state_x - goal_x) < 1e-3 && fabs(state_y - goal_y) < 1e-3 && fabs(state_z - goal_z) < 1e-3) {
+    1221             : 
+    1222             :       {
+    1223           2 :         std::scoped_lock lock(mutex_state_);
+    1224             : 
+    1225           2 :         state_x_ = goal_x;
+    1226           2 :         state_y_ = goal_y;
+    1227           2 :         state_z_ = goal_z;
+    1228             :       }
+    1229             : 
+    1230           2 :       changeState(IDLE_STATE);
+    1231             : 
+    1232           2 :       have_goal_ = false;
+    1233             :     }
+    1234             :   }
+    1235             : 
+    1236             :   {
+    1237        1791 :     std::scoped_lock lock(mutex_state_);
+    1238             : 
+    1239        1791 :     state_x_ += cos(current_heading_) * current_horizontal_speed_ * _tracker_dt_;
+    1240        1791 :     state_y_ += sin(current_heading_) * current_horizontal_speed_ * _tracker_dt_;
+    1241        1791 :     state_z_ += current_vertical_direction_ * current_vertical_speed_ * _tracker_dt_;
+    1242             :   }
+    1243             : 
+    1244             :   // --------------------------------------------------------------
+    1245             :   // |                        heading tracking                        |
+    1246             :   // --------------------------------------------------------------
+    1247             : 
+    1248             :   {
+    1249        3582 :     std::scoped_lock lock(mutex_state_);
+    1250             : 
+    1251             :     // compute the desired heading rate
+    1252             :     double current_heading_rate;
+    1253        1791 :     if (fabs(goal_heading_ - state_heading_) > M_PI)
+    1254           0 :       current_heading_rate = -_heading_gain_ * (goal_heading_ - state_heading_);
+    1255             :     else
+    1256        1791 :       current_heading_rate = _heading_gain_ * (goal_heading_ - state_heading_);
+    1257             : 
+    1258        1791 :     if (current_heading_rate > _heading_rate_) {
+    1259          50 :       current_heading_rate = _heading_rate_;
+    1260        1741 :     } else if (current_heading_rate < -_heading_rate_) {
+    1261           0 :       current_heading_rate = -_heading_rate_;
+    1262             :     }
+    1263             : 
+    1264             :     // flap the resulted state_heading_ aroud PI
+    1265        1791 :     state_heading_ += current_heading_rate * _tracker_dt_;
+    1266             : 
+    1267        1791 :     if (fabs(state_heading_ - goal_heading_) < (2 * (_heading_rate_ * _tracker_dt_))) {
+    1268        1352 :       state_heading_ = goal_heading_;
+    1269             :     }
+    1270             :   }
+    1271             : }
+    1272             : 
+    1273             : //}
+    1274             : 
+    1275             : }  // namespace line_tracker
+    1276             : 
+    1277             : }  // namespace mrs_uav_trackers
+    1278             : 
+    1279             : #include <pluginlib/class_list_macros.h>
+    1280           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..7dfaf9ff8e --- /dev/null +++ b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.overview.html @@ -0,0 +1,340 @@ + + + + + + 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 + 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..4455f1cdd74881c327aea5434192c633f0f8a8d0 GIT binary patch literal 3795 zcmV;^4lMDBP)dMDAPL^1%2P&I2;=B^Yo)DTctM}}+zMJX^xRa6LJ8mTt7SW0m^D>?Nee0t1D zv=_}WQg;>lW`&-+d8?&3l|5DrRI;9@qTjft?#FvTTRVd+d0%(VkaZq7fc0c-{A`{s zpVyjmM&N``Y&@1OI9R$#Rr|#wyi0TVs`T)?2H5#=ZlYAVt^lbchOA~zmw<$Ca z3HNkFQ(a*lMT58jKyfkpGsDenD=gA57=R_#A*N4R?hg1y?9pVM+%&q$9-)of zkz!ie7&uFTix@b!hK)2Y#1Kr9TpVL$JdL%ZSxbSxh$0Wez#L9qfX@JD0o|US;!!n8 zH8D_5kvFph$kiSJ#ljSTof`-BuJaPlq*_`=FuxuJEMdcNhV9Zl#eu__kozHA!F^h$ zI1Jdw#Hv_}f&J@PB)KGAS5MYidNJ#EqfCV1q$hywtfk{UDaB~65bL#mu27uI`agPZ z^mw!90Nhd>urGAZ-GAUNB<=tz>$tg} zu6ETF-!HZ;-z$v*p86>Ph2=VR1coI7?rRQM#X2KZ0_5~m%bFLvj^b|ptIRqJtYMww z9Z8)7N?F&9&2GeM-?*{aYMYoW?&D1pnyaNw=7fW#X2lM(r1sICRbT*KVYKI5;ZRU6 zls5H+&6Bf}P#Gwgz!{*p4B5(}^nQ1r59qTDcxYt4s;JkE+oD)`+EhVTtc z{BZ2wJn-z)APx8$vK9cPQ+q`dD$UF-af4;<8MoX}Gn*U8fbs=q7Fij3vm<+q2hRTO zZU!HpH;S9P8NlrOf;(ttB}H-hXuik!yKM@(1zLc#q18k)C-8P`a&znKdjNK~p0MAn zSVyTa@0k$qdkBcjzT4q-1M7$)3)E2b4xAC{QtLDQ7HB~J*pAMcM|*a(T3RvGQuL!o zHyTfY!YsFf^}r_Vc4tb`giNAMqwq(8mZymL+L8QsEh2ki7|WIklK?*@Oymk7{fl_1 zoa^BnHZE`iD5of2w3DvI*_L^x^KV?_3fHW~CHnBYC4!Z_KDsGZ8c2dLkLX44cPq89 zj!dSBS*jcWe3f!nHNu$k%w#@PKo6DxPf>C}S-GXTD-mm+cO`4vL}Y(0%w1KmF66F; zhvs}8rNX>U$1DTI;~sGtHty+o|KmL^JN2e8R|u0fIcu=gq-DcVHv1MCMjc@UTlNs=zl07W%XBz${siG#wJjY3HEAz!T!W>CJz zDmdFR9@uk1jM|$We|D9`P}r8xB|jvKJ+GSB6X(sJF;GFV^Ri1Y3O>)Q2L3z*#;aKx z`XwtqPY`=bOWi|`qx$=MPFaq6MkXGa{Ld(0_yfDJIl$mK)8mmlNifWm^W`kNPcm!|oss;!TK zVR?;x4DjVPTlDA)!3jg2< zh=vU;L5)>XvxkO&4L_gpO!Jxa4HU)mJ=^d@de@H~5FuKIw8@&5c}9DI@H^Bp;ZIRh zJ{}%;svXjRuOVvzaL8pFDlhzh9S|94W|5VlH%}^Mm%cr1?i-B4y_@6b?+i4G_DzVy zodxnHH2z*Xy5)>;1@+h=ttM0~eYK%9f#K1wDVoTSSdVAdAeKedf!aBlzDU|-)i;L|` z>2+>^2`S07Z1o|9h$a@Opy>8}r-urF4gZ^c&8@q+S4$&*OVlvnorxx34`ItO|J@MU z!2hcIq`;Yq0&wikeXgNb{VqFg*vp9CBkbYEzgXDo75eD$=UJF!>XZM<@h5g_|B6o{ zUu@_)a7s#PT%>TbO@dcA()dLb9`>7tMI@h0Zh3rJw`m*~P|POn5Jzf<3jEcsa!LovFW7Z*n9(~&mp4=u6q~bbWVQSA9MQjsIo82jsh)+Od4q^T~qlj(dYDUp( zg`Y5r#2|E1%g1IhWA4n^HChIGYw~o^=3b%4NQw>s=>gsA9Xf>vk4`3=XQ85qyS1G@ zyqsLayfP2g(p+&Yy3Kl}FIPox?EtC&eWcg$Lg;fn@-C#$5G{2*pTRBzJf^{CIG2fC zfprzd2&98_di!?tmc| zc)MXBG2|k|8ZhJ{DaMtt6lGHPzXBdU25t+#+*3jPFNK*&E4d2oOk))CCX@;z)g!sc zPOZ;K1L@^P0AD(4HvJ6NX4>IwxMQdN^>hX0(^&oapPbZoA!qabE-OqG>p9o)eM_$p zT?>MMBL^QTZy=>k;Y&E(H@h6;WUn`y++h)XhX&+}hZuNuFH|tT(H{bjnk3JN@Qro7 zp~8@vCimb|a;8ZMHDIPmQH<*1DaxcCI8;Cbg)La7eRa{H0^fvEVWfH_hYA$yUprLz z)?~q#guTxurK!v0EU?$aCtQy8!DtreACoGdKee}!g|Fu_nnJ0+DL#I7Dn-`u*T%;$ zol1R6eEjGF1Q#CrjrQ&U_$Bd?4Ug6FkqwW@ypX+AmW&iM@Rvr4Zbweid`O$rACDC9 zqhpfno_iM3D_UULAb+cw^*e@;oH%Hd`kTy*HXcnh`(4(9j}4;*xwc(R9q((#_6VPz zUc0T)rAUe`bC+CPJoE%a<}n_}`{*$qCl~UjV{IxvJl34d!`sv+USnrgh+B?Iw8oy* zkC2K$F~zk=O-m7`ko!<1uE_5|Q3jM!EQNVJ#XqUPU+P{I*RseJ6eYlsPi)9Vy}z(m zW-alA?~%HH0Qi%-k)jMJr8q9^m5#CMC`y0>!XCFdY(tw2h_zt^I6m0j-4E_TU*%rm z9u*EKN*59od7wy>H~GAInLLFDAJFoLBNmY!#|#Cf`KXBS$x0y$G}wqW9)9{*CU<-xnv7>t>coO z*_@RW#pQEtZKav~@$iYQ{sNB)@+n%>#rZr+diMrC?cxu*xEJAS+4J*45_Mo}oh{997mx?ux{{Y2Bxw4kq?nVFr002ov JPDHLkV1hT6WGVmv literal 0 HcmV?d00001 diff --git a/mrs_uav_trackers/src/midair_activation_tracker/index-detail-sort-f.html b/mrs_uav_trackers/src/midair_activation_tracker/index-detail-sort-f.html new file mode 100644 index 0000000000..a4de031456 --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/index-detail-sort-f.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-12-17 22:35:29Functions: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..61d8f5eda8 --- /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-12-17 22:35:29Functions: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..4fa41e1c28 --- /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-12-17 22:35:29Functions: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..8a6bffacda --- /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-12-17 22:35:29Functions: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..14b6e3a414 --- /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-12-17 22:35:29Functions: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..df7a3c19fa --- /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-12-17 22:35:29Functions: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..895f85d305 --- /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-12-17 22:35:29Functions: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()65
(anonymous namespace)::ProxyExec0::ProxyExec0()109
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>)109
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)160
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::deactivate()180
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)312
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)387
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)142109
+
+
+ + + +
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..99e336ae46 --- /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-12-17 22:35:29Functions:91850.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()109
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::deactivate()180
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>)109
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&)387
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)312
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&)142109
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)160
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::getStatus()65
+
+
+ + + +
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..642f18a066 --- /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-12-17 22:35:29Functions: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         109 : 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         109 :   this->common_handlers_  = common_handlers;
+      81         109 :   this->private_handlers_ = private_handlers;
+      82             : 
+      83         109 :   _uav_name_ = common_handlers->uav_name;
+      84             : 
+      85         109 :   nh_ = nh;
+      86             : 
+      87         109 :   ros::Time::waitForValid();
+      88             : 
+      89             :   // --------------------------------------------------------------
+      90             :   // |                     loading parameters                     |
+      91             :   // --------------------------------------------------------------
+      92             : 
+      93             :   // | ---------- loading params using the parent's nh ---------- |
+      94             : 
+      95         218 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+      96             : 
+      97         109 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+      98             : 
+      99         109 :   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         109 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/midair_activation_tracker.yaml");
+     107         109 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/midair_activation_tracker.yaml");
+     108             : 
+     109         218 :   const std::string yaml_prefix = "mrs_uav_trackers/midair_activation_tracker/";
+     110             : 
+     111         109 :   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         109 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "MidairActivationTracker", _profiler_enabled_);
+     119             : 
+     120             :   // | --------------------- finish the init -------------------- |
+     121             : 
+     122         109 :   is_initialized_ = true;
+     123             : 
+     124         109 :   ROS_INFO("[MidairActivationTracker]: initialized");
+     125             : 
+     126         109 :   return true;
+     127             : }
+     128             : 
+     129             : //}
+     130             : 
+     131             : /* //{ activate() */
+     132             : 
+     133         160 : std::tuple<bool, std::string> MidairActivationTracker::activate([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd) {
+     134             : 
+     135         160 :   std::stringstream ss;
+     136             : 
+     137         160 :   is_active_ = true;
+     138             : 
+     139         160 :   ss << "activated";
+     140         160 :   ROS_INFO_STREAM("[MidairActivationTracker]: " << ss.str());
+     141             : 
+     142         320 :   return std::tuple(true, ss.str());
+     143             : }
+     144             : 
+     145             : //}
+     146             : 
+     147             : /* //{ deactivate() */
+     148             : 
+     149         180 : void MidairActivationTracker::deactivate(void) {
+     150             : 
+     151         180 :   is_active_ = false;
+     152             : 
+     153         180 :   ROS_INFO("[MidairActivationTracker]: deactivated");
+     154         180 : }
+     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      142109 : 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      142109 :   if (!is_active_) {
+     174      141617 :     return {};
+     175             :   }
+     176             : 
+     177        1476 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     178             :   mrs_lib::ScopeTimer timer =
+     179        1476 :       mrs_lib::ScopeTimer("MidairActivationTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     180             : 
+     181         984 :   mrs_msgs::TrackerCommand tracker_cmd;
+     182             : 
+     183         492 :   tracker_cmd.header.frame_id = uav_state.header.frame_id;
+     184         492 :   tracker_cmd.header.stamp    = ros::Time::now();
+     185             : 
+     186         492 :   tracker_cmd.position.x = uav_state.pose.position.x;
+     187         492 :   tracker_cmd.position.y = uav_state.pose.position.y;
+     188         492 :   tracker_cmd.position.z = uav_state.pose.position.z;
+     189             : 
+     190         492 :   tracker_cmd.velocity.x = uav_state.velocity.linear.x;
+     191         492 :   tracker_cmd.velocity.y = uav_state.velocity.linear.y;
+     192         492 :   tracker_cmd.velocity.z = uav_state.velocity.linear.z;
+     193             : 
+     194             :   try {
+     195         492 :     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         492 :   tracker_cmd.use_position_vertical   = true;
+     203         492 :   tracker_cmd.use_position_horizontal = true;
+     204             : 
+     205         492 :   tracker_cmd.use_velocity_vertical   = true;
+     206         492 :   tracker_cmd.use_velocity_horizontal = true;
+     207             : 
+     208         492 :   tracker_cmd.use_heading = true;
+     209             : 
+     210         492 :   ROS_WARN_THROTTLE(0.1, "[MidairActivationTracker]: outputting cmd");
+     211             : 
+     212         492 :   return {tracker_cmd};
+     213             : }
+     214             : 
+     215             : //}
+     216             : 
+     217             : /* //{ getStatus() */
+     218             : 
+     219          65 : const mrs_msgs::TrackerStatus MidairActivationTracker::getStatus() {
+     220             : 
+     221          65 :   mrs_msgs::TrackerStatus tracker_status;
+     222             : 
+     223          65 :   tracker_status.active            = is_active_;
+     224          65 :   tracker_status.callbacks_enabled = callbacks_enabled_;
+     225             : 
+     226          65 :   return tracker_status;
+     227             : }
+     228             : 
+     229             : //}
+     230             : 
+     231             : /* //{ enableCallbacks() */
+     232             : 
+     233         312 : const std_srvs::SetBoolResponse::ConstPtr MidairActivationTracker::enableCallbacks([[maybe_unused]] const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     234             : 
+     235         624 :   std_srvs::SetBoolResponse res;
+     236             : 
+     237         312 :   res.message = "callbacks are always disabled";
+     238         312 :   res.success = true;
+     239             : 
+     240         624 :   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         387 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr MidairActivationTracker::setConstraints([
+     298             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     299             : 
+     300         774 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     301             : 
+     302         387 :   res.success = true;
+     303         387 :   res.message = "constraints updated";
+     304             : 
+     305         774 :   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         109 : 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..6b39ae3ce9 --- /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:1395168282.9 %
Date:2024-12-17 22:35:29Functions:455090.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.9%82.9%
+ 		82.9 %1395 / 168290.0 %45 / 50
<unnamed>82.9 %1395 / 168290.0 %45 / 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..f366fe56ab --- /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:1395168282.9 %
Date:2024-12-17 22:35:29Functions:455090.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.9%82.9%
+ 		82.9 %1395 / 168290.0 %45 / 50
<unnamed>82.9 %1395 / 168290.0 %45 / 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..4abf95e115 --- /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:1395168282.9 %
Date:2024-12-17 22:35:29Functions:455090.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.9%82.9%
+ 		82.9 %1395 / 168290.0 %45 / 50
<unnamed>82.9 %1395 / 168290.0 %45 / 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..05c52d6ff7 --- /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:1395168282.9 %
Date:2024-12-17 22:35:29Functions:455090.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.9%82.9%
+ 		82.9 %1395 / 168290.0 %45 / 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..069d0a29d8 --- /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:1395168282.9 %
Date:2024-12-17 22:35:29Functions:455090.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.9%82.9%
+ 		82.9 %1395 / 168290.0 %45 / 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..f690cb410d --- /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:1395168282.9 %
Date:2024-12-17 22:35:29Functions:455090.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.9%82.9%
+ 		82.9 %1395 / 168290.0 %45 / 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..d618a2a171 --- /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:1395168282.9 %
Date:2024-12-17 22:35:29Functions:455090.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::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::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)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&)100
(anonymous namespace)::ProxyExec0::ProxyExec0()109
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>)109
mrs_uav_trackers::mpc_tracker::MpcTracker::dynamicReconfigureCallback(mrs_uav_trackers::mpc_trackerConfig&, unsigned int)109
mrs_uav_trackers::mpc_tracker::MpcTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)269
mrs_uav_trackers::mpc_tracker::MpcTracker::setGoal(double, double, double, double, bool)271
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackOtherMavTrajectory(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)348
mrs_uav_trackers::mpc_tracker::MpcTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)387
mrs_uav_trackers::mpc_tracker::MpcTracker::timerHover(ros::TimerEvent const&)469
mrs_uav_trackers::mpc_tracker::MpcTracker::setRelativeGoal(double, double, double, double, bool)569
mrs_uav_trackers::mpc_tracker::MpcTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)809
mrs_uav_trackers::mpc_tracker::MpcTracker::timerVelocityTracking(ros::TimerEvent const&)821
mrs_uav_trackers::mpc_tracker::MpcTracker::loadTrajectory[abi:cxx11](mrs_msgs::TrajectoryReference_<std::allocator<void> >)824
mrs_uav_trackers::mpc_tracker::MpcTracker::setSinglePointReference(double, double, double, double)840
mrs_uav_trackers::mpc_tracker::MpcTracker::toggleHover(bool)1319
mrs_uav_trackers::mpc_tracker::MpcTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)2156
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackOtherMavDiagnostics(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)2189
mrs_uav_trackers::mpc_tracker::MpcTracker::timerAvoidanceTrajectory(ros::TimerEvent const&)4545
mrs_uav_trackers::mpc_tracker::MpcTracker::getStatus()9441
mrs_uav_trackers::mpc_tracker::MpcTracker::increaseCurrentTrajectoryTime(double)15783
mrs_uav_trackers::mpc_tracker::MpcTracker::timerDiagnostics(ros::TimerEvent const&)22909
mrs_uav_trackers::mpc_tracker::MpcTracker::publishDiagnostics()24619
mrs_uav_trackers::mpc_tracker::MpcTracker::getCurrentTrajectoryIdx()25225
mrs_uav_trackers::mpc_tracker::MpcTracker::iterateModel(double const&)81105
mrs_uav_trackers::mpc_tracker::MpcTracker::checkTrajectoryForCollisions(int&)83422
mrs_uav_trackers::mpc_tracker::MpcTracker::calculateMPC()84480
mrs_uav_trackers::mpc_tracker::MpcTracker::filterReferenceZ(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, double, double)84480
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)84480
mrs_uav_trackers::mpc_tracker::MpcTracker::manageConstraints()84480
mrs_uav_trackers::mpc_tracker::MpcTracker::timerMPC(ros::TimerEvent const&)84480
mrs_uav_trackers::mpc_tracker::MpcTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)142109
mrs_uav_trackers::mpc_tracker::MpcTracker::checkCollision(double, double, double, double, double, double)187680
mrs_uav_trackers::mpc_tracker::MpcTracker::checkCollisionInflated(double, double, double, double, double, double)187680
+
+
+ + + +
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..904940af6d --- /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:1395168282.9 %
Date:2024-12-17 22:35:29Functions:455090.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()109
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>)109
mrs_uav_trackers::mpc_tracker::MpcTracker::timerHover(ros::TimerEvent const&)469
mrs_uav_trackers::mpc_tracker::MpcTracker::resetStatic()0
mrs_uav_trackers::mpc_tracker::MpcTracker::toggleHover(bool)1319
mrs_uav_trackers::mpc_tracker::MpcTracker::calculateMPC()84480
mrs_uav_trackers::mpc_tracker::MpcTracker::iterateModel(double const&)81105
mrs_uav_trackers::mpc_tracker::MpcTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)269
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)187680
mrs_uav_trackers::mpc_tracker::MpcTracker::loadTrajectory[abi:cxx11](mrs_msgs::TrajectoryReference_<std::allocator<void> >)824
mrs_uav_trackers::mpc_tracker::MpcTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)387
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&)2156
mrs_uav_trackers::mpc_tracker::MpcTracker::setRelativeGoal(double, double, double, double, bool)569
mrs_uav_trackers::mpc_tracker::MpcTracker::filterReferenceZ(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, double, double)84480
mrs_uav_trackers::mpc_tracker::MpcTracker::timerDiagnostics(ros::TimerEvent const&)22909
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)84480
mrs_uav_trackers::mpc_tracker::MpcTracker::manageConstraints()84480
mrs_uav_trackers::mpc_tracker::MpcTracker::publishDiagnostics()24619
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&)809
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&)821
mrs_uav_trackers::mpc_tracker::MpcTracker::checkCollisionInflated(double, double, double, double, double, double)187680
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()25225
mrs_uav_trackers::mpc_tracker::MpcTracker::gotoTrajectoryStartImpl[abi:cxx11]()2
mrs_uav_trackers::mpc_tracker::MpcTracker::setSinglePointReference(double, double, double, double)840
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&)4545
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackOtherMavTrajectory(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)348
mrs_uav_trackers::mpc_tracker::MpcTracker::dynamicReconfigureCallback(mrs_uav_trackers::mpc_trackerConfig&, unsigned int)109
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>)2189
mrs_uav_trackers::mpc_tracker::MpcTracker::startTrajectoryTrackingImpl[abi:cxx11]()2
mrs_uav_trackers::mpc_tracker::MpcTracker::checkTrajectoryForCollisions(int&)83422
mrs_uav_trackers::mpc_tracker::MpcTracker::resumeTrajectoryTrackingImpl[abi:cxx11]()1
mrs_uav_trackers::mpc_tracker::MpcTracker::increaseCurrentTrajectoryTime(double)15783
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&)1
mrs_uav_trackers::mpc_tracker::MpcTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)142109
mrs_uav_trackers::mpc_tracker::MpcTracker::setGoal(double, double, double, double, bool)271
mrs_uav_trackers::mpc_tracker::MpcTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)100
mrs_uav_trackers::mpc_tracker::MpcTracker::timerMPC(ros::TimerEvent const&)84480
mrs_uav_trackers::mpc_tracker::MpcTracker::getStatus()9441
+
+
+ + + +
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..69c88d4a1e --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.html @@ -0,0 +1,3993 @@ + + + + + + + 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:1395168282.9 %
Date:2024-12-17 22:35:29Functions:455090.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             :   ros::Time time_last_update_;
+     123             : 
+     124             :   bool is_active_      = false;
+     125             :   bool is_initialized_ = false;
+     126             : 
+     127             :   // | ----------------------- constraints ---------------------- |
+     128             : 
+     129             :   mrs_msgs::DynamicsConstraints constraints_;
+     130             :   std::mutex                    mutex_constraints_;
+     131             : 
+     132             :   mrs_msgs::DynamicsConstraints constraints_filtered_;
+     133             :   std::mutex                    mutex_constraints_filtered_;
+     134             : 
+     135             :   std::atomic<bool> got_constraints_     = false;
+     136             :   std::atomic<bool> all_constraints_set_ = false;
+     137             : 
+     138             :   double _diag_pos_tracking_thr_;
+     139             :   double _diag_heading_tracking_thr_;
+     140             : 
+     141             :   double _mpc_synchronous_rate_limit_;
+     142             :   double _mpc_asynchronous_rate_;
+     143             : 
+     144             :   double update_rate_ = 100.0;
+     145             : 
+     146             :   double     dt1_;
+     147             :   std::mutex mutex_dt1_;
+     148             : 
+     149             :   double _dt2_;
+     150             : 
+     151             :   MatrixXd          _mat_A_;  // system matrix for virtual UAV
+     152             :   MatrixXd          _mat_B_;  // input matrix for virtual UAV
+     153             :   MatrixXd          A_;       // system matrix for virtual UAV
+     154             :   MatrixXd          B_;       // input matrix for virtual UAV
+     155             :   std::atomic<bool> model_first_iteration_ = true;
+     156             :   ros::Time         model_iteration_last_time_;
+     157             : 
+     158             :   MatrixXd _mat_A_heading_;  // system matrix for heading
+     159             :   MatrixXd _mat_B_heading_;  // input matrix for heading
+     160             :   MatrixXd A_heading_;       // system matrix for heading
+     161             :   MatrixXd B_heading_;       // input matrix for heading
+     162             : 
+     163             :   // the reference over the prediction horizon per axis
+     164             :   MatrixXd   des_x_trajectory_;
+     165             :   MatrixXd   des_y_trajectory_;
+     166             :   MatrixXd   des_z_trajectory_;
+     167             :   MatrixXd   des_heading_trajectory_;
+     168             :   std::mutex mutex_des_trajectory_;
+     169             : 
+     170             :   // the reference filtered over the prediction horizon per axis
+     171             :   MatrixXd des_z_filtered_offset_;
+     172             : 
+     173             :   // the whole trajectory reference split per axis
+     174             :   std::shared_ptr<VectorXd> des_x_whole_trajectory_;
+     175             :   std::shared_ptr<VectorXd> des_y_whole_trajectory_;
+     176             :   std::shared_ptr<VectorXd> des_z_whole_trajectory_;
+     177             :   std::shared_ptr<VectorXd> des_heading_whole_trajectory_;
+     178             :   int                       des_whole_trajectory_id_ = 0;
+     179             :   std::mutex                mutex_des_whole_trajectory_;
+     180             : 
+     181             :   int  getCurrentTrajectoryIdx();
+     182             :   void increaseCurrentTrajectoryTime(const double dt);
+     183             : 
+     184             :   // trajectory tracking
+     185             :   std::atomic<bool> trajectory_tracking_in_progress_ = false;
+     186             :   double            trajectory_current_time_;
+     187             :   std::mutex        mutex_trajectory_tracking_states_;
+     188             : 
+     189             :   // params of the loaded trajectory
+     190             :   int    trajectory_size_          = 0;
+     191             :   double trajectory_dt_            = 0.2;
+     192             :   bool   trajectory_track_heading_ = false;
+     193             :   bool   trajectory_tracking_loop_ = false;
+     194             :   bool   trajectory_set_           = false;
+     195             :   int    trajectory_count_         = 0;  // counts how many trajectories we have received
+     196             : 
+     197             :   // mpc output
+     198             :   VectorXd   mpc_u_;
+     199             :   double     mpc_u_heading_;
+     200             :   std::mutex mutex_mpc_u_;
+     201             : 
+     202             :   // current state of the dynamical system
+     203             :   MatrixXd   mpc_x_;          // translation state
+     204             :   MatrixXd   mpc_x_heading_;  // heading state
+     205             :   std::mutex mutex_mpc_x_;
+     206             : 
+     207             :   // odometry reset
+     208             :   std::atomic<bool> odometry_reset_in_progress_ = false;
+     209             :   std::atomic<bool> mpc_result_invalid_         = false;
+     210             : 
+     211             :   // predicting the future
+     212             :   MatrixXd   predicted_trajectory_;
+     213             :   MatrixXd   predicted_heading_trajectory_;
+     214             :   std::mutex mutex_predicted_trajectory_;
+     215             : 
+     216             :   mrs_msgs::MpcPredictionFullState prediction_full_state_;
+     217             :   std::mutex                       mutex_prediction_full_state_;
+     218             : 
+     219             :   mrs_lib::PublisherHandler<geometry_msgs::PoseArray>   ph_predicted_trajectory_debugging_;
+     220             :   mrs_lib::PublisherHandler<geometry_msgs::PoseArray>   ph_mpc_reference_debugging_;
+     221             :   mrs_lib::PublisherHandler<geometry_msgs::PoseStamped> ph_current_trajectory_point_;
+     222             :   mrs_lib::PublisherHandler<geometry_msgs::PoseStamped> ph_first_reference_point_;
+     223             : 
+     224             :   std::atomic<bool> mpc_computed_ = false;
+     225             : 
+     226             :   bool brake_ = false;
+     227             : 
+     228             :   // | ----------------------- MPC solver ----------------------- |
+     229             : 
+     230             :   std::shared_ptr<mrs_mpc_solvers::mpc_tracker::Solver> mpc_solver_y_;
+     231             :   std::shared_ptr<mrs_mpc_solvers::mpc_tracker::Solver> mpc_solver_x_;
+     232             :   std::shared_ptr<mrs_mpc_solvers::mpc_tracker::Solver> mpc_solver_z_;
+     233             :   std::shared_ptr<mrs_mpc_solvers::mpc_tracker::Solver> mpc_solver_heading_;
+     234             : 
+     235             :   std::mutex mutex_mpc_calculation_;
+     236             : 
+     237             :   int _max_iters_xy_;
+     238             :   int _max_iters_z_;
+     239             :   int _max_iters_heading_;
+     240             : 
+     241             :   // | ----------- measuring the "MPC realtime factor" ---------- |
+     242             : 
+     243             :   double mpc_rtf_ = 0.0;
+     244             : 
+     245             :   // | ------------------- collision avoidance ------------------ |
+     246             : 
+     247             :   // configurable params
+     248             :   bool collision_avoidance_enabled_           = false;
+     249             :   bool collision_avoidance_enabled_passively_ = true;
+     250             : 
+     251             :   // TODO what is this?
+     252             :   double    coef_scaler = 0;
+     253             :   ros::Time coef_time;
+     254             : 
+     255             :   double minimum_collison_free_altitude_ = std::numeric_limits<double>::lowest();
+     256             : 
+     257             :   // params
+     258             :   double                   _avoidance_trajectory_rate_;
+     259             :   double                   _avoidance_radius_threshold_;
+     260             :   double                   _avoidance_z_correction_;
+     261             :   std::string              _avoidance_diagnostics_topic_name_;
+     262             :   std::vector<std::string> _avoidance_other_uav_names_;
+     263             :   double                   _avoidance_z_threshold_;
+     264             : 
+     265             :   // how old can the other UAV trajectory be (since receive time)
+     266             :   double _collision_trajectory_timeout_;
+     267             : 
+     268             :   // when collision detected, slow down during the manouver
+     269             :   double _avoidance_collision_horizontal_speed_coef_;
+     270             : 
+     271             :   // when collision detected, slow down fully this number of steps before it
+     272             :   int _avoidance_collision_slow_down_fully_;
+     273             : 
+     274             :   // when collision detected, start slowing down this number of steps before it
+     275             :   int _avoidance_collision_slow_down_;
+     276             : 
+     277             :   // when avoiding, start climbing this number of steps before it
+     278             :   int _avoidance_collision_start_climbing_;
+     279             : 
+     280             :   int avoidance_this_uav_number_;
+     281             :   int avoidance_this_uav_priority_;
+     282             : 
+     283             :   double            collision_free_altitude_;
+     284             :   std::atomic<bool> avoiding_collision_               = false;
+     285             :   bool              collision_avoidance_affecting_me_ = false;
+     286             : 
+     287             :   // avoidance trajectory will not be published unless we computed it at least once
+     288             :   std::atomic<bool> future_was_predicted_ = false;
+     289             : 
+     290             :   // subscribing to the other UAV future trajectories
+     291             :   void callbackOtherMavTrajectory(const mrs_msgs::FutureTrajectory::ConstPtr msg);
+     292             : 
+     293             :   std::vector<mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory>> other_uav_trajectory_subscribers_;
+     294             :   std::map<std::string, mrs_msgs::FutureTrajectory>                  other_uav_avoidance_trajectories_;
+     295             :   std::mutex                                                         mutex_other_uav_avoidance_trajectories_;
+     296             : 
+     297             :   // subscribing to the other UAV diagnostics'
+     298             :   void callbackOtherMavDiagnostics(const mrs_msgs::MpcTrackerDiagnostics::ConstPtr msg);
+     299             : 
+     300             :   std::vector<mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics>> other_uav_diag_subscribers_;
+     301             :   std::map<std::string, mrs_msgs::MpcTrackerDiagnostics>                  other_uav_diagnostics_;
+     302             :   std::mutex                                                              mutex_other_uav_diagnostics_;
+     303             : 
+     304             :   bool checkCollision(const double ax, const double ay, const double az, const double bx, const double by, const double bz);
+     305             :   bool checkCollisionInflated(const double ax, const double ay, const double az, const double bx, const double by, const double bz);
+     306             : 
+     307             :   mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory> ph_avoidance_trajectory_;
+     308             : 
+     309             :   ros::ServiceServer service_server_toggle_avoidance_;
+     310             :   bool               callbackToggleCollisionAvoidance(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     311             : 
+     312             :   mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics> sh_estimation_diag_;
+     313             : 
+     314             :   // | --------------------- MPC calculation -------------------- |
+     315             : 
+     316             :   ros::Timer        timer_mpc_iteration_;
+     317             :   std::atomic<bool> mpc_synchronous_ = false;
+     318             :   ros::TimerEvent   synchronous_timer_event_;
+     319             : 
+     320             :   std::atomic<bool> mpc_timer_running_ = false;
+     321             :   void              timerMPC(const ros::TimerEvent& event);
+     322             : 
+     323             :   // | -------------------- velocity tracking ------------------- |
+     324             : 
+     325             :   ros::Timer                  timer_velocity_tracking_;
+     326             :   void                        timerVelocityTracking(const ros::TimerEvent& event);
+     327             :   ros::Time                   velocity_reference_time_;
+     328             :   mrs_msgs::VelocityReference velocity_reference_;
+     329             :   std::mutex                  mutex_velocity_reference_;
+     330             :   std::atomic<bool>           velocity_tracking_active_ = false;
+     331             : 
+     332             :   // | ------------------ avoidance trajectory ------------------ |
+     333             : 
+     334             :   ros::Timer timer_avoidance_trajectory_;
+     335             :   void       timerAvoidanceTrajectory(const ros::TimerEvent& event);
+     336             : 
+     337             :   // | ----------------------- diagnostics ---------------------- |
+     338             : 
+     339             :   ros::Timer timer_diagnostics_;
+     340             :   double     _diagnostics_rate_;
+     341             :   void       timerDiagnostics(const ros::TimerEvent& event);
+     342             : 
+     343             :   // | ------------------------ hovering ------------------------ |
+     344             : 
+     345             :   ros::Timer        timer_hover_;
+     346             :   void              timerHover(const ros::TimerEvent& event);
+     347             :   std::atomic<bool> hovering_in_progress_ = false;
+     348             :   void              toggleHover(bool in);
+     349             : 
+     350             :   // | ------------------- trajectory tracking ------------------ |
+     351             : 
+     352             :   std::tuple<bool, std::string> resumeTrajectoryTrackingImpl(void);
+     353             :   std::tuple<bool, std::string> startTrajectoryTrackingImpl(void);
+     354             :   std::tuple<bool, std::string> stopTrajectoryTrackingImpl(void);
+     355             :   std::tuple<bool, std::string> gotoTrajectoryStartImpl(void);
+     356             : 
+     357             :   // | --------------------- other routines --------------------- |
+     358             : 
+     359             :   void publishDiagnostics();
+     360             : 
+     361             :   void debugPrintState(const double throttle);
+     362             :   void debugPrintMPCResult(const double throttle);
+     363             : 
+     364             :   void setGoal(const double pos_x, const double pos_y, const double pos_z, const double heading, const bool use_heading);
+     365             :   void setRelativeGoal(const double pos_x, const double pos_y, const double pos_z, const double heading, const bool use_heading);
+     366             :   void setSinglePointReference(const double x, const double y, const double z, const double heading);
+     367             : 
+     368             :   std::tuple<bool, std::string, bool> loadTrajectory(const mrs_msgs::TrajectoryReference msg);
+     369             : 
+     370             :   MatrixXd                       filterReferenceZ(const VectorXd& des_z_trajectory, const double max_ascending_speed, const double max_descending_speed);
+     371             :   std::tuple<MatrixXd, MatrixXd> filterReferenceXY(const VectorXd& des_x_trajectory, const VectorXd& des_y_trajectory, double max_speed_x, double max_speed_y);
+     372             : 
+     373             :   double checkTrajectoryForCollisions(int& first_collision_index);
+     374             : 
+     375             :   void manageConstraints(void);
+     376             :   void calculateMPC(void);
+     377             :   void iterateModel(const double& dt);
+     378             : 
+     379             :   // | ------------------------ profiler ------------------------ |
+     380             : 
+     381             :   mrs_lib::Profiler profiler;
+     382             :   bool              _profiler_enabled_ = false;
+     383             : 
+     384             :   // | ------------------------- wiggle ------------------------- |
+     385             : 
+     386             :   ros::ServiceServer service_server_wiggle_;
+     387             :   bool               callbackWiggle(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     388             : 
+     389             :   double wiggle_phase_ = 0;
+     390             : 
+     391             :   // | --------------- dynamic reconfigure server --------------- |
+     392             : 
+     393             :   void dynamicReconfigureCallback(mrs_uav_trackers::mpc_trackerConfig& config, uint32_t level);
+     394             : 
+     395             :   boost::recursive_mutex                      config_mutex_;
+     396             :   typedef mrs_uav_trackers::mpc_trackerConfig Config;
+     397             :   typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
+     398             :   boost::shared_ptr<ReconfigureServer>        reconfigure_server_;
+     399             :   mrs_uav_trackers::mpc_trackerConfig         drs_params_;
+     400             :   std::mutex                                  mutex_drs_params_;
+     401             : };
+     402             : 
+     403             : //}
+     404             : 
+     405             : // | -------------- tracker's interface routines -------------- |
+     406             : 
+     407             : /* //{ initialize() */
+     408             : 
+     409         109 : bool MpcTracker::initialize(const ros::NodeHandle& nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+     410             :                             std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers) {
+     411             : 
+     412         109 :   nh_ = nh;
+     413             : 
+     414         109 :   common_handlers_  = common_handlers;
+     415         109 :   private_handlers_ = private_handlers;
+     416             : 
+     417         109 :   _uav_name_ = common_handlers->uav_name;
+     418             : 
+     419         109 :   ros::Time::waitForValid();
+     420             : 
+     421         109 :   time_last_update_ = ros::Time(0);
+     422             : 
+     423             :   // --------------------------------------------------------------
+     424             :   // |                     loading parameters                     |
+     425             :   // --------------------------------------------------------------
+     426             : 
+     427             :   // | ---------- loading params using the parent's nh ---------- |
+     428             : 
+     429         218 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     430             : 
+     431         109 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     432             : 
+     433         109 :   if (!param_loader_parent.loadedSuccessfully()) {
+     434           0 :     ROS_ERROR("[MpcTracker]: Could not load all parameters!");
+     435           0 :     return false;
+     436             :   }
+     437             : 
+     438             :   // | --------------- loading plugin's parameters -------------- |
+     439             : 
+     440         109 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/mpc_tracker.yaml");
+     441         109 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/mpc_tracker.yaml");
+     442             : 
+     443         218 :   const std::string yaml_prefix = "mrs_uav_trackers/mpc_tracker/";
+     444             : 
+     445         109 :   private_handlers->param_loader->loadParam("network/robot_names", _avoidance_other_uav_names_);
+     446             : 
+     447         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_loop/synchronous_rate_limit", _mpc_synchronous_rate_limit_);
+     448         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_loop/asynchronous_loop_rate", _mpc_asynchronous_rate_);
+     449             : 
+     450         109 :   if (_mpc_asynchronous_rate_ < 15) {
+     451           0 :     ROS_ERROR("[MpcTracker]: the asynchronous_loop_rate must be > 15 Hz");
+     452           0 :     return false;
+     453             :   }
+     454             : 
+     455         109 :   dt1_ = 1.0 / _mpc_asynchronous_rate_;
+     456             : 
+     457         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "braking/enabled", drs_params_.braking_enabled);
+     458         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "braking/q_vel_braking", drs_params_.q_vel_braking);
+     459         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "braking/q_vel_no_braking", drs_params_.q_vel_no_braking);
+     460             : 
+     461         109 :   private_handlers->param_loader->loadMatrixKnown(yaml_prefix + "model/translation/A", _mat_A_, MPC_N_STATES, MPC_N_STATES);
+     462         109 :   private_handlers->param_loader->loadMatrixKnown(yaml_prefix + "model/translation/B", _mat_B_, MPC_N_STATES, MPC_N_INPUTS);
+     463             : 
+     464         109 :   A_ = _mat_A_;
+     465         109 :   B_ = _mat_B_;
+     466             : 
+     467         109 :   private_handlers->param_loader->loadMatrixKnown(yaml_prefix + "model/heading/A", _mat_A_heading_, MPC_HEADING_N_STATES, MPC_HEADING_N_STATES);
+     468         109 :   private_handlers->param_loader->loadMatrixKnown(yaml_prefix + "model/heading/B", _mat_B_heading_, MPC_HEADING_N_STATES, MPC_HEADING_N_INPUTS);
+     469             : 
+     470         109 :   A_heading_ = _mat_A_heading_;
+     471         109 :   B_heading_ = _mat_B_heading_;
+     472             : 
+     473             :   // load the MPC parameters
+     474         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/dt2", _dt2_);
+     475             : 
+     476         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "diagnostics/rate", _diagnostics_rate_);
+     477         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "diagnostics/position_tracking_threshold", _diag_pos_tracking_thr_);
+     478         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "diagnostics/orientation_tracking_threshold", _diag_heading_tracking_thr_);
+     479             : 
+     480         109 :   bool verbose_xy      = false;
+     481         109 :   bool verbose_z       = false;
+     482         109 :   bool verbose_heading = false;
+     483             : 
+     484         218 :   std::vector<double> xy_Q;
+     485         218 :   std::vector<double> z_Q;
+     486         218 :   std::vector<double> heading_Q;
+     487             : 
+     488         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/xy/verbose", verbose_xy);
+     489         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/xy/max_n_iterations", _max_iters_xy_);
+     490         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/xy/Q", xy_Q);
+     491             : 
+     492         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/z/verbose", verbose_z);
+     493         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/z/max_n_iterations", _max_iters_z_);
+     494         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/z/Q", z_Q);
+     495             : 
+     496         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/heading/verbose", verbose_heading);
+     497         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/heading/max_n_iterations", _max_iters_heading_);
+     498         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/heading/Q", heading_Q);
+     499             : 
+     500         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "wiggle/enabled", drs_params_.wiggle_enabled);
+     501         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "wiggle/amplitude", drs_params_.wiggle_amplitude);
+     502         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "wiggle/frequency", drs_params_.wiggle_frequency);
+     503             : 
+     504         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/enabled", collision_avoidance_enabled_);
+     505         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/enabled_passively", collision_avoidance_enabled_passively_);
+     506         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/predicted_trajectory_publish_rate", _avoidance_trajectory_rate_);
+     507         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/correction", _avoidance_z_correction_);
+     508         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/radius", _avoidance_radius_threshold_);
+     509         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/altitude_threshold", _avoidance_z_threshold_);
+     510         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/collision_horizontal_speed_coef", _avoidance_collision_horizontal_speed_coef_);
+     511         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/collision_slow_down_fully", _avoidance_collision_slow_down_fully_);
+     512         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/collision_slow_down_start", _avoidance_collision_slow_down_);
+     513         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/collision_start_climbing", _avoidance_collision_start_climbing_);
+     514         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/trajectory_timeout", _collision_trajectory_timeout_);
+     515             : 
+     516         109 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     517           0 :     ROS_ERROR("[MpcTracker]: could not load all parameters!");
+     518           0 :     return false;
+     519             :   }
+     520             : 
+     521         109 :   ROS_INFO_STREAM("[MpcTracker]: initializing solvers with dt1 = " << dt1_);
+     522             : 
+     523         109 :   mpc_solver_y_ = std::make_shared<mrs_mpc_solvers::mpc_tracker::Solver>("MpcTracker_y", verbose_xy, _max_iters_xy_, xy_Q, dt1_, _dt2_, 1);
+     524         109 :   mpc_solver_x_ = std::make_shared<mrs_mpc_solvers::mpc_tracker::Solver>("MpcTracker_x", verbose_xy, _max_iters_xy_, xy_Q, dt1_, _dt2_, 0);
+     525         109 :   mpc_solver_z_ = std::make_shared<mrs_mpc_solvers::mpc_tracker::Solver>("MpcTracker_z", verbose_z, _max_iters_z_, z_Q, dt1_, _dt2_, 2);
+     526             :   mpc_solver_heading_ =
+     527         109 :       std::make_shared<mrs_mpc_solvers::mpc_tracker::Solver>("MpcTracker_hdg", verbose_heading, _max_iters_heading_, heading_Q, dt1_, _dt2_, 0);
+     528             : 
+     529         109 :   mpc_x_         = MatrixXd::Zero(MPC_N_STATES, 1);
+     530         109 :   mpc_x_heading_ = MatrixXd::Zero(MPC_HEADING_N_STATES, 1);
+     531             : 
+     532         109 :   mpc_u_ = VectorXd::Zero(MPC_N_INPUTS);
+     533             : 
+     534         109 :   coef_time = ros::Time(0);
+     535             : 
+     536         109 :   des_x_trajectory_       = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+     537         109 :   des_y_trajectory_       = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+     538         109 :   des_z_trajectory_       = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+     539         109 :   des_z_filtered_offset_  = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+     540         109 :   des_heading_trajectory_ = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+     541             : 
+     542         109 :   service_server_wiggle_ = nh_.advertiseService("wiggle", &MpcTracker::callbackWiggle, this);
+     543             : 
+     544         109 :   pub_diagnostics_   = mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics>(nh_, "diagnostics", 1);
+     545         109 :   pub_status_string_ = mrs_lib::PublisherHandler<std_msgs::String>(nh_, "string", 1);
+     546             : 
+     547             :   // extract the numerical name
+     548         109 :   sscanf(_uav_name_.c_str(), "uav%d", &avoidance_this_uav_number_);
+     549         109 :   ROS_INFO("[MpcTracker]: Numerical ID of this UAV is %d", avoidance_this_uav_number_);
+     550         109 :   avoidance_this_uav_priority_ = avoidance_this_uav_number_;
+     551             : 
+     552             :   // exclude this drone from the list
+     553         109 :   std::vector<std::string>::iterator it = _avoidance_other_uav_names_.begin();
+     554         228 :   while (it != _avoidance_other_uav_names_.end()) {
+     555             : 
+     556         119 :     std::string temp_str = *it;
+     557             : 
+     558             :     int other_uav_priority;
+     559         119 :     sscanf(temp_str.c_str(), "uav%d", &other_uav_priority);
+     560             : 
+     561         119 :     if (other_uav_priority == avoidance_this_uav_number_) {
+     562             : 
+     563         109 :       _avoidance_other_uav_names_.erase(it);
+     564         109 :       continue;
+     565             :     }
+     566             : 
+     567          10 :     it++;
+     568             :   }
+     569             : 
+     570             :   // initialize velocity tracker
+     571             : 
+     572         109 :   velocity_reference_time_ = ros::Time(0);
+     573             : 
+     574             :   // create publishers for predicted trajectory
+     575             : 
+     576         109 :   ph_avoidance_trajectory_           = mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory>(nh_, "predicted_trajectory", 1);
+     577         109 :   ph_predicted_trajectory_debugging_ = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "predicted_trajectory_debugging", 1);
+     578         109 :   ph_mpc_reference_debugging_        = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "mpc_reference_debugging", 1, true);
+     579         109 :   ph_current_trajectory_point_       = mrs_lib::PublisherHandler<geometry_msgs::PoseStamped>(nh_, "current_trajectory_point", 1, true);
+     580         109 :   ph_first_reference_point_          = mrs_lib::PublisherHandler<geometry_msgs::PoseStamped>(nh_, "first_reference_point", 1, true);
+     581             : 
+     582         109 :   pub_debug_processed_trajectory_poses_   = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "trajectory_processed/poses", 1, true);
+     583         109 :   pub_debug_processed_trajectory_markers_ = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "trajectory_processed/markers", 1, true);
+     584             : 
+     585             :   // preallocate predicted trajectory
+     586         109 :   predicted_trajectory_         = MatrixXd::Zero(MPC_HORIZON_LENGTH * MPC_N_STATES, 1);
+     587         109 :   predicted_heading_trajectory_ = MatrixXd::Zero(MPC_HORIZON_LENGTH * MPC_N_STATES, 1);
+     588             : 
+     589         109 :   collision_free_altitude_ = std::numeric_limits<float>::lowest();
+     590             : 
+     591             :   // collision avoidance toggle service
+     592         109 :   service_server_toggle_avoidance_ = nh_.advertiseService("collision_avoidance", &MpcTracker::callbackToggleCollisionAvoidance, this);
+     593             : 
+     594         218 :   mrs_lib::SubscribeHandlerOptions shopts;
+     595         109 :   shopts.nh                 = nh_;
+     596         109 :   shopts.node_name          = "MpcTracker";
+     597         109 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     598         109 :   shopts.threadsafe         = true;
+     599         109 :   shopts.autostart          = true;
+     600         109 :   shopts.queue_size         = 10;
+     601         109 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     602             : 
+     603             :   // create subscribers on other drones diagnostics
+     604         109 :   if (collision_avoidance_enabled_ || collision_avoidance_enabled_passively_) {
+     605             : 
+     606         119 :     for (int i = 0; i < int(_avoidance_other_uav_names_.size()); i++) {
+     607             : 
+     608          30 :       std::string prediction_topic_name = std::string("/") + _avoidance_other_uav_names_.at(i) + "/control_manager/mpc_tracker/predicted_trajectory";
+     609          20 :       std::string diag_topic_name       = std::string("/") + _avoidance_other_uav_names_.at(i) + "/control_manager/mpc_tracker/diagnostics";
+     610             : 
+     611          10 :       ROS_INFO("[MpcTracker]: subscribing to %s", prediction_topic_name.c_str());
+     612             : 
+     613          10 :       other_uav_trajectory_subscribers_.push_back(
+     614          20 :           mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory>(shopts, prediction_topic_name, &MpcTracker::callbackOtherMavTrajectory, this));
+     615             : 
+     616          10 :       ROS_INFO("[MpcTracker]: subscribing to %s", diag_topic_name.c_str());
+     617             : 
+     618          10 :       other_uav_diag_subscribers_.push_back(
+     619          20 :           mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics>(shopts, diag_topic_name, &MpcTracker::callbackOtherMavDiagnostics, this));
+     620             :     }
+     621             :   }
+     622             : 
+     623         109 :   sh_estimation_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, std::string("/") + _uav_name_ + "/estimation_manager/diagnostics");
+     624             : 
+     625             :   // | --------------- dynamic reconfigure server --------------- |
+     626             : 
+     627         109 :   reconfigure_server_.reset(new ReconfigureServer(config_mutex_, nh_));
+     628         109 :   reconfigure_server_->updateConfig(drs_params_);
+     629         109 :   ReconfigureServer::CallbackType f = boost::bind(&MpcTracker::dynamicReconfigureCallback, this, _1, _2);
+     630         109 :   reconfigure_server_->setCallback(f);
+     631             : 
+     632             :   // | ------------------------ profiler ------------------------ |
+     633             : 
+     634         109 :   profiler = mrs_lib::Profiler(common_handlers->parent_nh, "MpcTracker", _profiler_enabled_);
+     635             : 
+     636             :   // | ------------------------- timers ------------------------- |
+     637             : 
+     638         218 :   timer_avoidance_trajectory_ = nh_.createTimer(ros::Rate(_avoidance_trajectory_rate_), &MpcTracker::timerAvoidanceTrajectory, this, false,
+     639         218 :                                                 collision_avoidance_enabled_ || collision_avoidance_enabled_passively_);
+     640         109 :   timer_diagnostics_          = nh_.createTimer(ros::Rate(_diagnostics_rate_), &MpcTracker::timerDiagnostics, this);
+     641         109 :   timer_mpc_iteration_        = nh_.createTimer(ros::Rate(_mpc_asynchronous_rate_), &MpcTracker::timerMPC, this, false, false);
+     642         109 :   timer_velocity_tracking_    = nh_.createTimer(ros::Rate(30.0), &MpcTracker::timerVelocityTracking, this, false, false);
+     643         109 :   timer_hover_                = nh_.createTimer(ros::Rate(10.0), &MpcTracker::timerHover, this, false, false);
+     644             : 
+     645             :   // | ----------------------- finish init ---------------------- |
+     646             : 
+     647         109 :   is_initialized_ = true;
+     648             : 
+     649         109 :   ROS_INFO("[MpcTracker]: initialized");
+     650             : 
+     651         109 :   return true;
+     652             : }
+     653             : 
+     654             : //}
+     655             : 
+     656             : /* //{ activate() */
+     657             : 
+     658         100 : std::tuple<bool, std::string> MpcTracker::activate(const std::optional<mrs_msgs::TrackerCommand>& last_tracker_cmd) {
+     659             : 
+     660         200 :   std::stringstream ss;
+     661             : 
+     662         100 :   if (!got_constraints_) {
+     663             : 
+     664           0 :     ss << "can not activate, missing constraints";
+     665           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+     666             : 
+     667           0 :     return std::tuple(false, ss.str());
+     668             :   }
+     669             : 
+     670         200 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     671             : 
+     672             :   double uav_state_heading;
+     673             : 
+     674             :   try {
+     675         100 :     uav_state_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     676             :   }
+     677           0 :   catch (...) {
+     678           0 :     ss << "could not calculate the UAV heading";
+     679           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+     680           0 :     return std::tuple(false, ss.str());
+     681             :   }
+     682             : 
+     683         200 :   MatrixXd mpc_x         = MatrixXd::Zero(MPC_N_STATES, 1);
+     684         100 :   MatrixXd mpc_x_heading = MatrixXd::Zero(MPC_HEADING_N_STATES, 1);
+     685             : 
+     686         100 :   if (last_tracker_cmd) {
+     687             : 
+     688             :     // set the initial condition from the last tracker's cmd
+     689             : 
+     690          98 :     if (last_tracker_cmd->use_position_horizontal) {
+     691          98 :       mpc_x(0, 0) = last_tracker_cmd->position.x;
+     692          98 :       mpc_x(4, 0) = last_tracker_cmd->position.y;
+     693             :     } else {
+     694           0 :       mpc_x(0, 0) = uav_state.pose.position.x;
+     695           0 :       mpc_x(4, 0) = uav_state.pose.position.y;
+     696             :     }
+     697             : 
+     698          98 :     if (last_tracker_cmd->use_position_vertical) {
+     699          98 :       mpc_x(8, 0) = last_tracker_cmd->position.z;
+     700             :     } else {
+     701           0 :       mpc_x(8, 0) = uav_state.pose.position.z;
+     702             :     }
+     703             : 
+     704          98 :     if (last_tracker_cmd->use_velocity_horizontal) {
+     705          98 :       mpc_x(1, 0) = last_tracker_cmd->velocity.x;
+     706          98 :       mpc_x(5, 0) = last_tracker_cmd->velocity.y;
+     707             :     } else {
+     708           0 :       mpc_x(1, 0) = uav_state.velocity.linear.x;
+     709           0 :       mpc_x(5, 0) = uav_state.velocity.linear.y;
+     710             :     }
+     711             : 
+     712          98 :     if (last_tracker_cmd->use_velocity_vertical) {
+     713          98 :       mpc_x(9, 0) = last_tracker_cmd->velocity.z;
+     714             :     } else {
+     715           0 :       mpc_x(9, 0) = uav_state.velocity.linear.z;
+     716             :     }
+     717             : 
+     718          98 :     if (last_tracker_cmd->use_acceleration) {
+     719           0 :       mpc_x(2, 0)  = last_tracker_cmd->acceleration.x;
+     720           0 :       mpc_x(6, 0)  = last_tracker_cmd->acceleration.y;
+     721           0 :       mpc_x(10, 0) = last_tracker_cmd->acceleration.z;
+     722             :     } else {
+     723          98 :       mpc_x(2, 0)  = 0;
+     724          98 :       mpc_x(6, 0)  = 0;
+     725          98 :       mpc_x(10, 0) = 0;
+     726             :     }
+     727             : 
+     728             :     // the jerks
+     729          98 :     mpc_x(3, 0)  = 0;
+     730          98 :     mpc_x(7, 0)  = 0;
+     731          98 :     mpc_x(11, 0) = 0;
+     732             : 
+     733          98 :     if (last_tracker_cmd->use_heading) {
+     734          98 :       mpc_x_heading(0, 0) = last_tracker_cmd->heading;
+     735           0 :     } else if (last_tracker_cmd->use_orientation) {
+     736             :       try {
+     737           0 :         mpc_x_heading(0, 0) = mrs_lib::AttitudeConverter(last_tracker_cmd->orientation).getHeading();
+     738             :       }
+     739           0 :       catch (...) {
+     740           0 :         mpc_x_heading(0, 0) = uav_state_heading;
+     741             :       }
+     742             :     } else {
+     743           0 :       mpc_x_heading(0, 0) = uav_state_heading;
+     744             :     }
+     745             : 
+     746          98 :     if (last_tracker_cmd->use_heading_rate) {
+     747          21 :       mpc_x_heading(1, 0) = last_tracker_cmd->heading_rate;
+     748             :     } else {
+     749          77 :       mpc_x_heading(1, 0) = uav_state.velocity.angular.z;
+     750             :     }
+     751             : 
+     752          98 :     mpc_x_heading(2, 0) = 0;
+     753          98 :     mpc_x_heading(3, 0) = 0;
+     754             : 
+     755          98 :     ROS_INFO("[MpcTracker]: activated with last tracker's command");
+     756             : 
+     757             :   } else {
+     758             : 
+     759             :     // set the initial condition completely from the uav_state
+     760             : 
+     761           2 :     mpc_x(0, 0) = uav_state.pose.position.x;
+     762           2 :     mpc_x(1, 0) = uav_state.velocity.linear.x;
+     763           2 :     mpc_x(2, 0) = 0;
+     764           2 :     mpc_x(3, 0) = 0;
+     765             : 
+     766           2 :     mpc_x(4, 0) = uav_state.pose.position.y;
+     767           2 :     mpc_x(5, 0) = uav_state.velocity.linear.y;
+     768           2 :     mpc_x(6, 0) = 0;
+     769           2 :     mpc_x(7, 0) = 0;
+     770             : 
+     771           2 :     mpc_x(8, 0)  = uav_state.pose.position.z;
+     772           2 :     mpc_x(9, 0)  = uav_state.velocity.linear.z;
+     773           2 :     mpc_x(10, 0) = 0;
+     774           2 :     mpc_x(11, 0) = 0;
+     775             : 
+     776           2 :     mpc_x_heading(0, 0) = uav_state_heading;
+     777           2 :     mpc_x_heading(1, 0) = uav_state.velocity.angular.z;
+     778           2 :     mpc_x_heading(2, 0) = 0;
+     779           2 :     mpc_x_heading(3, 0) = 0;
+     780             : 
+     781           2 :     ROS_INFO("[MpcTracker]: activated with uav state");
+     782             :   }
+     783             : 
+     784             :   {
+     785         200 :     std::scoped_lock lock(mutex_mpc_x_);
+     786             : 
+     787         100 :     mpc_x_         = mpc_x;
+     788         100 :     mpc_x_heading_ = mpc_x_heading;
+     789             :   }
+     790             : 
+     791         100 :   trajectory_tracking_in_progress_ = false;
+     792             : 
+     793         100 :   ss << "activated";
+     794         100 :   ROS_INFO_STREAM("[MpcTracker]: " << ss.str());
+     795             : 
+     796             :   // this is here to initialize the desired_trajectory vector
+     797             :   // if deleted (and I tried) the UAV will briefly fly to the
+     798             :   // origin after activation
+     799         100 :   setRelativeGoal(0, 0, 0, 0, false);  // do not delete
+     800             : 
+     801         100 :   toggleHover(true);
+     802             : 
+     803         100 :   model_first_iteration_ = true;
+     804             : 
+     805         100 :   A_ = _mat_A_;
+     806         100 :   B_ = _mat_B_;
+     807             : 
+     808         100 :   A_heading_ = _mat_A_heading_;
+     809         100 :   B_heading_ = _mat_B_heading_;
+     810             : 
+     811         100 :   is_active_ = true;
+     812             : 
+     813         100 :   if (!mpc_synchronous_) {
+     814          91 :     timer_mpc_iteration_.start();
+     815             :   }
+     816             : 
+     817         100 :   return std::tuple(true, ss.str());
+     818             : }
+     819             : 
+     820             : //}
+     821             : 
+     822             : /* //{ deactivate() */
+     823             : 
+     824          40 : void MpcTracker::deactivate(void) {
+     825             : 
+     826          40 :   toggleHover(false);
+     827             : 
+     828          40 :   is_active_                       = false;
+     829          40 :   trajectory_tracking_in_progress_ = false;
+     830          40 :   model_first_iteration_           = true;
+     831             : 
+     832          40 :   time_last_update_ = ros::Time(0);
+     833             : 
+     834             :   {
+     835          40 :     std::scoped_lock lock(mutex_trajectory_tracking_states_);
+     836             : 
+     837          40 :     trajectory_current_time_ = 0;
+     838             :   }
+     839             : 
+     840          40 :   ROS_INFO("[MpcTracker]: deactivated");
+     841             : 
+     842          40 :   timer_mpc_iteration_.stop();
+     843             : 
+     844          40 :   publishDiagnostics();
+     845          40 : }
+     846             : 
+     847             : //}
+     848             : 
+     849             : /* //{ resetStatic() */
+     850             : 
+     851           0 : bool MpcTracker::resetStatic(void) {
+     852             : 
+     853           0 :   if (!is_initialized_) {
+     854           0 :     ROS_ERROR("[MpcTracker]: can not reset, not initialized");
+     855           0 :     return false;
+     856             :   }
+     857             : 
+     858           0 :   if (!is_active_) {
+     859           0 :     ROS_ERROR("[MpcTracker]: can not reset, not active");
+     860           0 :     return false;
+     861             :   }
+     862             : 
+     863           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     864             : 
+     865             :   double uav_state_heading;
+     866             : 
+     867             :   try {
+     868           0 :     uav_state_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     869             :   }
+     870           0 :   catch (...) {
+     871           0 :     ROS_ERROR_THROTTLE(1.0, "[MpcTracker]: could not calculate the UAV heading");
+     872           0 :     return false;
+     873             :   }
+     874             : 
+     875             :   {
+     876           0 :     std::scoped_lock lock(mutex_mpc_x_);
+     877             : 
+     878             :     // set the initial condition from the odometry
+     879             : 
+     880           0 :     ROS_INFO("[MpcTracker]: reseting with uav state with no dynamics");
+     881             : 
+     882           0 :     mpc_x_(0, 0) = uav_state.pose.position.x;
+     883           0 :     mpc_x_(1, 0) = 0;
+     884           0 :     mpc_x_(2, 0) = 0;
+     885           0 :     mpc_x_(3, 0) = 0;
+     886             : 
+     887           0 :     mpc_x_(4, 0) = uav_state.pose.position.y;
+     888           0 :     mpc_x_(5, 0) = 0;
+     889           0 :     mpc_x_(6, 0) = 0;
+     890           0 :     mpc_x_(7, 0) = 0;
+     891             : 
+     892           0 :     mpc_x_(8, 0)  = uav_state.pose.position.z;
+     893           0 :     mpc_x_(9, 0)  = 0;
+     894           0 :     mpc_x_(10, 0) = 0;
+     895           0 :     mpc_x_(11, 0) = 0;
+     896             : 
+     897           0 :     mpc_x_heading_(0, 0) = uav_state_heading;
+     898           0 :     mpc_x_heading_(1, 0) = 0;
+     899           0 :     mpc_x_heading_(2, 0) = 0;
+     900           0 :     mpc_x_heading_(3, 0) = 0;
+     901             : 
+     902           0 :     trajectory_tracking_in_progress_ = false;
+     903             : 
+     904           0 :     ROS_INFO("[MpcTracker]: reseted");
+     905             :   }
+     906             : 
+     907             :   // this is here to initialize the desired_trajectory vector
+     908             :   // if deleted (and I tried) the UAV will briefly fly to the
+     909             :   // origin after activation
+     910           0 :   setRelativeGoal(0, 0, 0, 0, false);  // do not delete
+     911             : 
+     912           0 :   return true;
+     913             : }
+     914             : 
+     915             : //}
+     916             : 
+     917             : /* //{ update() */
+     918             : 
+     919      142109 : std::optional<mrs_msgs::TrackerCommand> MpcTracker::update(const mrs_msgs::UavState&                                           uav_state,
+     920             :                                                            [[maybe_unused]] const mrs_uav_managers::Controller::ControlOutput& last_control_output) {
+     921             : 
+     922      426327 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("update");
+     923      426327 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("MpcTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     924             : 
+     925      284218 :   auto old_uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     926             : 
+     927             :   // save the uav state
+     928      142109 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     929             : 
+     930             :   // the time from the last update call
+     931      142109 :   double dt = 0.01;
+     932             : 
+     933      142109 :   if (time_last_update_.isValid()) {
+     934      142109 :     dt = (ros::Time::now() - time_last_update_).toSec();
+     935             :   }
+     936             : 
+     937      142109 :   time_last_update_ = ros::Time::now();
+     938             : 
+     939      142109 :   if (dt > 0) {
+     940             : 
+     941      141873 :     double rate = 1.0 / dt;
+     942             : 
+     943      141873 :     update_rate_ = 0.9 * update_rate_ + 0.1 * rate;
+     944             : 
+     945      141873 :     if (mpc_synchronous_ && (update_rate_ > _mpc_synchronous_rate_limit_)) {
+     946           0 :       mpc_synchronous_ = false;
+     947           0 :       ROS_INFO("[MpcTracker]: detecting high update date (%.1f Hz > %.1f Hz), switching to asynchronous mode.", rate, _mpc_synchronous_rate_limit_);
+     948           0 :       if (is_active_) {
+     949           0 :         timer_mpc_iteration_.start();
+     950             :       }
+     951      141873 :     } else if (!mpc_synchronous_ && (update_rate_ <= _mpc_synchronous_rate_limit_)) {
+     952          14 :       mpc_synchronous_ = true;
+     953          14 :       ROS_INFO("[MpcTracker]: detecting low update rate (%.1f Hz < %.1f Hz), switching to synchronous mode.", rate, _mpc_synchronous_rate_limit_);
+     954          14 :       timer_mpc_iteration_.stop();
+     955             :     }
+     956             :   }
+     957             : 
+     958             :   // up to this part the update() method is evaluated even when the tracker is not active
+     959      142109 :   if (!is_active_) {
+     960       60754 :     return {};
+     961             :   }
+     962             : 
+     963      162710 :   mrs_msgs::TrackerCommand tracker_cmd;
+     964             : 
+     965       81355 :   if (!mpc_synchronous_ && (!mpc_computed_ || mpc_result_invalid_)) {
+     966             : 
+     967         138 :     ROS_WARN_THROTTLE(0.1, "[MpcTracker]: MPC not ready, returning current odom as the command");
+     968             : 
+     969             :     // set the header
+     970         138 :     tracker_cmd.header.stamp    = uav_state.header.stamp;
+     971         138 :     tracker_cmd.header.frame_id = uav_state.header.frame_id;
+     972             : 
+     973             :     // set positions from odom
+     974         138 :     tracker_cmd.position.x              = uav_state.pose.position.x;
+     975         138 :     tracker_cmd.position.y              = uav_state.pose.position.y;
+     976         138 :     tracker_cmd.position.z              = uav_state.pose.position.z;
+     977         138 :     tracker_cmd.use_position_vertical   = 1;
+     978         138 :     tracker_cmd.use_position_horizontal = 1;
+     979             : 
+     980             :     // set velocities from odom
+     981         138 :     tracker_cmd.velocity.x              = uav_state.velocity.linear.x;
+     982         138 :     tracker_cmd.velocity.y              = uav_state.velocity.linear.y;
+     983         138 :     tracker_cmd.velocity.z              = uav_state.velocity.linear.z;
+     984         138 :     tracker_cmd.use_velocity_vertical   = 1;
+     985         138 :     tracker_cmd.use_velocity_horizontal = 1;
+     986             : 
+     987             :     // set zero accelerations
+     988         138 :     tracker_cmd.acceleration.x   = 0;
+     989         138 :     tracker_cmd.acceleration.y   = 0;
+     990         138 :     tracker_cmd.acceleration.z   = 0;
+     991         138 :     tracker_cmd.use_acceleration = 1;
+     992             : 
+     993             :     try {
+     994         138 :       tracker_cmd.heading     = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     995         138 :       tracker_cmd.use_heading = 1;
+     996             :     }
+     997           0 :     catch (...) {
+     998           0 :       tracker_cmd.use_heading = 0;
+     999           0 :       ROS_WARN_THROTTLE(1.0, "[MpcTracker]: could not calculate the current UAV heading");
+    1000             :     }
+    1001             : 
+    1002             :     // set zero jerk
+    1003         138 :     tracker_cmd.jerk.x = 0;
+    1004         138 :     tracker_cmd.jerk.y = 0;
+    1005         138 :     tracker_cmd.jerk.z = 0;
+    1006             : 
+    1007             :     try {
+    1008         138 :       tracker_cmd.heading_rate     = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeadingRate(uav_state.velocity.angular);
+    1009         138 :       tracker_cmd.use_heading_rate = 1;
+    1010             :     }
+    1011           0 :     catch (...) {
+    1012           0 :       tracker_cmd.use_heading_rate = 0;
+    1013           0 :       ROS_WARN_THROTTLE(1.0, "[MpcTracker]: could not calculate the current UAV heading rate");
+    1014             :     }
+    1015             : 
+    1016         138 :     return {tracker_cmd};
+    1017             :   }
+    1018             : 
+    1019       81217 :   if (mpc_synchronous_) {
+    1020             : 
+    1021        4151 :     ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: running in SYNCHRONOUS mode");
+    1022             : 
+    1023        4151 :     if (synchronous_timer_event_.last_real.toSec() > 0) {
+    1024        4137 :       synchronous_timer_event_.last_real = synchronous_timer_event_.current_real;
+    1025             :     } else {
+    1026          14 :       synchronous_timer_event_.last_real = ros::Time::now();
+    1027             :     }
+    1028             : 
+    1029        4151 :     synchronous_timer_event_.current_real = ros::Time::now();
+    1030             : 
+    1031        4151 :     timerMPC(synchronous_timer_event_);
+    1032             : 
+    1033             :   } else {
+    1034       77066 :     ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: running in ASYNCHRONOUS mode");
+    1035             :   }
+    1036             : 
+    1037       81217 :   if (dt > 0) {
+    1038       81105 :     iterateModel(dt);
+    1039             :   } else {
+    1040         112 :     ROS_WARN_THROTTLE(1.0, "[MpcTracker]: dt !> 0, not iterating the model");
+    1041             :   }
+    1042             : 
+    1043      162434 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    1044      162434 :   auto prediction_full_state  = mrs_lib::get_mutexed(mutex_prediction_full_state_, prediction_full_state_);
+    1045             : 
+    1046             :   // check whether all outputs are finite
+    1047       81217 :   bool arefinite = true;
+    1048     1055821 :   for (int i = 0; i < 12; i++) {
+    1049      974604 :     if (!std::isfinite(mpc_x(i, 0))) {
+    1050           0 :       arefinite = false;
+    1051             :     }
+    1052             :   }
+    1053             : 
+    1054       81217 :   if (arefinite) {
+    1055             : 
+    1056             :     // set the desired states base on the result of the mpc
+    1057       81217 :     tracker_cmd.position.x     = mpc_x(0, 0);
+    1058       81217 :     tracker_cmd.velocity.x     = mpc_x(1, 0);
+    1059       81217 :     tracker_cmd.acceleration.x = mpc_x(2, 0);
+    1060       81217 :     tracker_cmd.jerk.x         = mpc_x(3, 0);
+    1061             : 
+    1062       81217 :     tracker_cmd.position.y     = mpc_x(4, 0);
+    1063       81217 :     tracker_cmd.velocity.y     = mpc_x(5, 0);
+    1064       81217 :     tracker_cmd.acceleration.y = mpc_x(6, 0);
+    1065       81217 :     tracker_cmd.jerk.y         = mpc_x(7, 0);
+    1066             : 
+    1067       81217 :     tracker_cmd.position.z     = mpc_x(8, 0);
+    1068       81217 :     tracker_cmd.velocity.z     = mpc_x(9, 0);
+    1069       81217 :     tracker_cmd.acceleration.z = mpc_x(10, 0);
+    1070       81217 :     tracker_cmd.jerk.z         = mpc_x(11, 0);
+    1071             : 
+    1072       81217 :     tracker_cmd.full_state_prediction = prediction_full_state;
+    1073             : 
+    1074       81217 :     tracker_cmd.use_position_vertical     = 1;
+    1075       81217 :     tracker_cmd.use_position_horizontal   = 1;
+    1076       81217 :     tracker_cmd.use_velocity_vertical     = 1;
+    1077       81217 :     tracker_cmd.use_velocity_horizontal   = 1;
+    1078       81217 :     tracker_cmd.use_acceleration          = 1;
+    1079       81217 :     tracker_cmd.use_jerk                  = 1;
+    1080       81217 :     tracker_cmd.use_full_state_prediction = 1;
+    1081             : 
+    1082             :   } else {
+    1083             : 
+    1084           0 :     ROS_ERROR_THROTTLE(1.0, "[MpcTracker]: MPC translation outputs are not finite!");
+    1085             : 
+    1086           0 :     return {};
+    1087             :   }
+    1088             : 
+    1089       81217 :   bool heading_finite = true;
+    1090      406085 :   for (int i = 0; i < MPC_HEADING_N_STATES; i++) {
+    1091      324868 :     if (!std::isfinite(mpc_x_heading(i, 0))) {
+    1092           0 :       heading_finite = false;
+    1093             :     }
+    1094             :   }
+    1095             : 
+    1096       81217 :   if (heading_finite) {
+    1097             : 
+    1098       81217 :     tracker_cmd.heading              = mpc_x_heading(0, 0);
+    1099       81217 :     tracker_cmd.heading_rate         = mpc_x_heading(1, 0);
+    1100       81217 :     tracker_cmd.heading_acceleration = mpc_x_heading(2, 0);
+    1101       81217 :     tracker_cmd.heading_jerk         = mpc_x_heading(3, 0);
+    1102             : 
+    1103       81217 :     tracker_cmd.use_heading              = 1;
+    1104       81217 :     tracker_cmd.use_heading_rate         = 1;
+    1105       81217 :     tracker_cmd.use_heading_acceleration = 1;
+    1106       81217 :     tracker_cmd.use_heading_jerk         = 1;
+    1107             : 
+    1108             :   } else {
+    1109             : 
+    1110           0 :     ROS_ERROR_THROTTLE(1.0, "[MpcTracker]: MPC heading output is not finite!");
+    1111             : 
+    1112           0 :     return {};
+    1113             :   }
+    1114             : 
+    1115             :   // set the header
+    1116       81217 :   tracker_cmd.header.stamp    = uav_state.header.stamp;
+    1117       81217 :   tracker_cmd.header.frame_id = uav_state.header.frame_id;
+    1118             : 
+    1119             :   // u have to return a position command
+    1120             :   // can set the jerk to 0
+    1121       81217 :   return {tracker_cmd};
+    1122             : }  // namespace mpc_tracker
+    1123             : 
+    1124             : //}
+    1125             : 
+    1126             : /* //{ getStatus() */
+    1127             : 
+    1128        9441 : const mrs_msgs::TrackerStatus MpcTracker::getStatus() {
+    1129             : 
+    1130       18882 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    1131        9441 :   auto trajectory_size        = mrs_lib::get_mutexed(mutex_des_trajectory_, trajectory_size_);
+    1132             : 
+    1133             :   double des_x, des_y, des_z, des_heading;
+    1134             :   {
+    1135        9441 :     std::scoped_lock lock(mutex_des_trajectory_);
+    1136             : 
+    1137        9441 :     des_x       = des_x_trajectory_(0);
+    1138        9441 :     des_y       = des_y_trajectory_(0);
+    1139        9441 :     des_z       = des_z_trajectory_(0);
+    1140        9441 :     des_heading = des_heading_trajectory_(0);
+    1141             :   }
+    1142             : 
+    1143        9441 :   mrs_msgs::TrackerStatus tracker_status;
+    1144             : 
+    1145        9441 :   tracker_status.active            = is_active_;
+    1146        9441 :   tracker_status.callbacks_enabled = is_active_ && callbacks_enabled_ && !hovering_in_progress_;
+    1147             : 
+    1148        9441 :   tracker_status.tracking_trajectory = trajectory_tracking_in_progress_;
+    1149             : 
+    1150        9441 :   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_;
+    1151        9441 :   bool have_heading_error    = fabs(radians::diff(mpc_x_heading(0), des_heading)) > _diag_heading_tracking_thr_;
+    1152        9441 :   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;
+    1153             : 
+    1154        9441 :   tracker_status.have_goal = trajectory_tracking_in_progress_ || hovering_in_progress_ || have_position_error || have_heading_error || have_nonzero_velocity;
+    1155             : 
+    1156        9441 :   if (!is_active_)
+    1157           0 :     tracker_status.state = mrs_msgs::TrackerStatus::STATE_IDLE;
+    1158        9441 :   else if (tracker_status.tracking_trajectory)
+    1159        1829 :     tracker_status.state = mrs_msgs::TrackerStatus::STATE_TRAJECTORY;
+    1160        7612 :   else if (tracker_status.have_goal)
+    1161        4272 :     tracker_status.state = mrs_msgs::TrackerStatus::STATE_REFERENCE;
+    1162             :   else
+    1163        3340 :     tracker_status.state = mrs_msgs::TrackerStatus::STATE_HOVER;
+    1164             : 
+    1165             : 
+    1166        9441 :   int trajectory_tracking_idx = getCurrentTrajectoryIdx();
+    1167             : 
+    1168        9441 :   tracker_status.trajectory_length = trajectory_size;
+    1169        9441 :   tracker_status.trajectory_idx    = trajectory_tracking_idx;
+    1170             : 
+    1171        9441 :   if (trajectory_tracking_in_progress_) {
+    1172             : 
+    1173        3658 :     auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1174             : 
+    1175        1829 :     std::scoped_lock lock(mutex_des_whole_trajectory_);
+    1176             : 
+    1177        1829 :     tracker_status.trajectory_reference.header.stamp    = ros::Time::now();
+    1178        1829 :     tracker_status.trajectory_reference.header.frame_id = uav_state.header.frame_id;
+    1179             : 
+    1180        1829 :     tracker_status.trajectory_reference.reference.position.x = (*des_x_whole_trajectory_)(trajectory_tracking_idx);
+    1181        1829 :     tracker_status.trajectory_reference.reference.position.y = (*des_y_whole_trajectory_)(trajectory_tracking_idx);
+    1182        1829 :     tracker_status.trajectory_reference.reference.position.z = (*des_z_whole_trajectory_)(trajectory_tracking_idx);
+    1183        1829 :     tracker_status.trajectory_reference.reference.heading    = (*des_heading_whole_trajectory_)(trajectory_tracking_idx);
+    1184             :   }
+    1185             : 
+    1186       18882 :   return tracker_status;
+    1187             : }
+    1188             : 
+    1189             : //}
+    1190             : 
+    1191             : /* //{ enableCallbacks() */
+    1192             : 
+    1193        2156 : const std_srvs::SetBoolResponse::ConstPtr MpcTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr& cmd) {
+    1194             : 
+    1195        4312 :   std::stringstream ss;
+    1196             : 
+    1197        2156 :   if (cmd->data != callbacks_enabled_) {
+    1198             : 
+    1199        1864 :     callbacks_enabled_ = cmd->data;
+    1200        1864 :     ss << "callbacks %s" << (callbacks_enabled_ ? "enabled" : "disabled");
+    1201             : 
+    1202             :   } else {
+    1203             : 
+    1204         292 :     ss << "callbacks were already %s" << (callbacks_enabled_ ? "enabled" : "disabled");
+    1205             :   }
+    1206             : 
+    1207        4312 :   std_srvs::SetBoolResponse res;
+    1208        2156 :   res.message = ss.str();
+    1209        2156 :   res.success = true;
+    1210             : 
+    1211        4312 :   return std_srvs::SetBoolResponse::ConstPtr(new std_srvs::SetBoolResponse(res));
+    1212             : }
+    1213             : 
+    1214             : //}
+    1215             : 
+    1216             : /* switchOdometrySource() //{ */
+    1217             : 
+    1218           5 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::switchOdometrySource(const mrs_msgs::UavState& new_uav_state) {
+    1219             : 
+    1220           5 :   odometry_reset_in_progress_ = true;
+    1221           5 :   mpc_result_invalid_         = true;
+    1222             : 
+    1223          10 :   auto x         = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    1224          10 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1225             : 
+    1226           5 :   ROS_INFO(
+    1227             :       "[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: "
+    1228             :       "%.2f], "
+    1229             :       "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]",
+    1230             :       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,
+    1231             :       new_uav_state.pose.position.z, new_uav_state.velocity.linear.x, new_uav_state.velocity.linear.y, new_uav_state.velocity.linear.z,
+    1232             :       new_uav_state.acceleration.linear.x, new_uav_state.acceleration.linear.y, new_uav_state.acceleration.linear.z);
+    1233             : 
+    1234           5 :   timer_mpc_iteration_.stop();
+    1235           5 :   ROS_INFO("[MpcTracker]: mpc timer stopped");
+    1236             : 
+    1237           5 :   while (mpc_timer_running_) {
+    1238             : 
+    1239           1 :     ROS_DEBUG("[MpcTracker]: the mpc is in the middle of an iteration, waiting for it to finish");
+    1240           1 :     ros::Duration wait(0.001);
+    1241           1 :     wait.sleep();
+    1242             : 
+    1243           1 :     if (!mpc_timer_running_) {
+    1244           1 :       ROS_DEBUG("[MpcTracker]: mpc timer finished");
+    1245           1 :       break;
+    1246             :     }
+    1247             :   }
+    1248             : 
+    1249             :   // | --------- recalculate the goal to new coordinates -------- |
+    1250             : 
+    1251           5 :   double old_heading  = 0;
+    1252           5 :   double new_heading  = 0;
+    1253           5 :   bool   got_headings = true;
+    1254             : 
+    1255             :   try {
+    1256           5 :     old_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1257             :   }
+    1258           0 :   catch (...) {
+    1259           0 :     ROS_ERROR_THROTTLE(1.0, "[LineTracker]: could not calculate the old UAV heading");
+    1260           0 :     got_headings = false;
+    1261             :   }
+    1262             : 
+    1263             :   try {
+    1264           5 :     new_heading = mrs_lib::AttitudeConverter(new_uav_state.pose.orientation).getHeading();
+    1265             :   }
+    1266           0 :   catch (...) {
+    1267           0 :     ROS_ERROR_THROTTLE(1.0, "[LineTracker]: could not calculate the new UAV heading");
+    1268           0 :     got_headings = false;
+    1269             :   }
+    1270             : 
+    1271          10 :   std_srvs::TriggerResponse res;
+    1272             : 
+    1273           5 :   if (!got_headings) {
+    1274           0 :     res.message = "could not calculate the heading difference";
+    1275           0 :     res.success = false;
+    1276             : 
+    1277           0 :     return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1278             :   }
+    1279             : 
+    1280             :   // calculate the difference of position
+    1281           5 :   double dx       = new_uav_state.pose.position.x - uav_state_.pose.position.x;
+    1282           5 :   double dy       = new_uav_state.pose.position.y - uav_state_.pose.position.y;
+    1283           5 :   double dz       = new_uav_state.pose.position.z - uav_state_.pose.position.z;
+    1284           5 :   double dheading = new_heading - old_heading;
+    1285             : 
+    1286           5 :   ROS_INFO("[MpcTracker]: dx %f dy %f dz %f dheading %f", dx, dy, dz, dheading);
+    1287             : 
+    1288             :   {
+    1289           5 :     std::scoped_lock lock(mutex_mpc_x_, mutex_des_trajectory_, mutex_des_whole_trajectory_, mutex_uav_state_);
+    1290             : 
+    1291           5 :     if (trajectory_set_) {
+    1292             : 
+    1293           0 :       for (int i = 0; i < trajectory_size_ + MPC_HORIZON_LENGTH; i++) {
+    1294             : 
+    1295           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);
+    1296           0 :         temp_vec = Eigen::Rotation2D<double>(dheading).toRotationMatrix() * temp_vec;
+    1297             : 
+    1298           0 :         (*des_x_whole_trajectory_)(i) = new_uav_state.pose.position.x + temp_vec(0);
+    1299           0 :         (*des_y_whole_trajectory_)(i) = new_uav_state.pose.position.y + temp_vec(1);
+    1300           0 :         (*des_z_whole_trajectory_)(i) += dz;
+    1301           0 :         (*des_heading_whole_trajectory_)(i) += dheading;
+    1302             :       }
+    1303             :     }
+    1304             : 
+    1305         205 :     for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    1306             : 
+    1307         200 :       Eigen::Vector2d temp_vec(des_x_trajectory_(i) - uav_state_.pose.position.x, des_y_trajectory_(i) - uav_state_.pose.position.y);
+    1308         200 :       temp_vec = Eigen::Rotation2D<double>(dheading).toRotationMatrix() * temp_vec;
+    1309             : 
+    1310         200 :       des_x_trajectory_(i, 0) = new_uav_state.pose.position.x + temp_vec(0);
+    1311         200 :       des_y_trajectory_(i, 0) = new_uav_state.pose.position.y + temp_vec(1);
+    1312         200 :       des_z_trajectory_(i, 0) += dz;
+    1313         200 :       des_heading_trajectory_(i, 0) += dheading;
+    1314             :     }
+    1315             : 
+    1316             :     // update the position
+    1317             :     {
+    1318           5 :       Eigen::Vector2d temp_vec(mpc_x_(0, 0) - uav_state_.pose.position.x, mpc_x_(4, 0) - uav_state_.pose.position.y);
+    1319           5 :       temp_vec     = Eigen::Rotation2D<double>(dheading).toRotationMatrix() * temp_vec;
+    1320           5 :       mpc_x_(0, 0) = new_uav_state.pose.position.x + temp_vec(0);
+    1321           5 :       mpc_x_(4, 0) = new_uav_state.pose.position.y + temp_vec(1);
+    1322           5 :       mpc_x_(8, 0) += dz;
+    1323             :     }
+    1324             : 
+    1325             :     // update the velocity
+    1326             :     {
+    1327           5 :       mpc_x_(1, 0) = new_uav_state.velocity.linear.x;
+    1328           5 :       mpc_x_(5, 0) = new_uav_state.velocity.linear.y;
+    1329             :       // we leave the z velocity as it was in the original frame
+    1330             :     }
+    1331             : 
+    1332             :     // update the acceleration
+    1333             :     {
+    1334           5 :       mpc_x_(2, 0)  = 0;
+    1335           5 :       mpc_x_(6, 0)  = 0;
+    1336           5 :       mpc_x_(10, 0) = 0;
+    1337             :     }
+    1338             : 
+    1339             :     // update the heading and its derivative
+    1340           5 :     mpc_x_heading_(0, 0) += dheading;
+    1341           5 :     mpc_x_heading_(1, 0) = new_uav_state.velocity.angular.x;
+    1342             :   }
+    1343             : 
+    1344           5 :   ROS_INFO(
+    1345             :       "[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: "
+    1346             :       "%.2f]",
+    1347             :       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));
+    1348             : 
+    1349           5 :   ROS_INFO("[MpcTracker]: starting the MPC timer");
+    1350             : 
+    1351           5 :   if (!mpc_synchronous_) {
+    1352           5 :     timer_mpc_iteration_.start();
+    1353             :   }
+    1354             : 
+    1355           5 :   odometry_reset_in_progress_ = false;
+    1356             : 
+    1357           5 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1358             : }
+    1359             : 
+    1360             : //}
+    1361             : 
+    1362             : /* //{ hover() */
+    1363             : 
+    1364           1 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::hover([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1365             : 
+    1366           1 :   toggleHover(true);
+    1367             : 
+    1368           2 :   std::stringstream ss;
+    1369           1 :   ss << "initiating hover";
+    1370             : 
+    1371           2 :   std_srvs::TriggerResponse res;
+    1372           1 :   res.success = true;
+    1373           1 :   res.message = ss.str();
+    1374             : 
+    1375           2 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1376             : }
+    1377             : 
+    1378             : //}
+    1379             : 
+    1380             : /* //{ startTrajectoryTracking() */
+    1381             : 
+    1382           2 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::startTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1383           4 :   std::stringstream ss;
+    1384             : 
+    1385           4 :   auto [success, message] = startTrajectoryTrackingImpl();
+    1386             : 
+    1387           4 :   std_srvs::TriggerResponse res;
+    1388           2 :   res.success = success;
+    1389           2 :   res.message = message;
+    1390             : 
+    1391           4 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1392             : }
+    1393             : 
+    1394             : //}
+    1395             : 
+    1396             : /* //{ stopTrajectoryTracking() */
+    1397             : 
+    1398           1 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::stopTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1399             : 
+    1400           2 :   auto [success, message] = stopTrajectoryTrackingImpl();
+    1401             : 
+    1402           2 :   std_srvs::TriggerResponse res;
+    1403           1 :   res.success = success;
+    1404           1 :   res.message = message;
+    1405             : 
+    1406           2 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1407             : }
+    1408             : 
+    1409             : //}
+    1410             : 
+    1411             : /* //{ resumeTrajectoryTracking() */
+    1412             : 
+    1413           1 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::resumeTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1414             : 
+    1415           2 :   auto [success, message] = resumeTrajectoryTrackingImpl();
+    1416             : 
+    1417           2 :   std_srvs::TriggerResponse res;
+    1418           1 :   res.success = success;
+    1419           1 :   res.message = message;
+    1420             : 
+    1421           2 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1422             : }
+    1423             : 
+    1424             : //}
+    1425             : 
+    1426             : /* //{ gotoTrajectoryStart() */
+    1427             : 
+    1428           2 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::gotoTrajectoryStart([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1429             : 
+    1430           4 :   auto [success, message] = gotoTrajectoryStartImpl();
+    1431             : 
+    1432           4 :   std_srvs::TriggerResponse res;
+    1433           2 :   res.success = success;
+    1434           2 :   res.message = message;
+    1435             : 
+    1436           4 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1437             : }
+    1438             : 
+    1439             : //}
+    1440             : 
+    1441             : /* //{ setConstraints() */
+    1442             : 
+    1443         387 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr MpcTracker::setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& constraints) {
+    1444             : 
+    1445         387 :   if (!is_initialized_) {
+    1446           0 :     return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+    1447             :   }
+    1448             : 
+    1449         387 :   mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_);
+    1450             : 
+    1451             :   // directly updated the speeds in the constraints
+    1452             :   // the reset needs to wait for manageConstraints()
+    1453             :   {
+    1454         774 :     std::scoped_lock lock(mutex_constraints_filtered_);
+    1455             : 
+    1456             :     // important! this needs to be done to initialize the full struct
+    1457         387 :     if (!got_constraints_) {
+    1458             : 
+    1459         109 :       constraints_filtered_ = constraints->constraints;
+    1460             : 
+    1461             :     } else {
+    1462             : 
+    1463         278 :       constraints_filtered_.horizontal_speed          = constraints->constraints.horizontal_speed;
+    1464         278 :       constraints_filtered_.vertical_ascending_speed  = constraints->constraints.vertical_ascending_speed;
+    1465         278 :       constraints_filtered_.vertical_descending_speed = constraints->constraints.vertical_descending_speed;
+    1466         278 :       constraints_filtered_.heading_speed             = constraints->constraints.heading_speed;
+    1467             :     }
+    1468             :   }
+    1469             : 
+    1470         387 :   got_constraints_ = true;
+    1471             : 
+    1472         387 :   all_constraints_set_ = false;
+    1473             : 
+    1474         387 :   ROS_INFO("[MpcTracker]: updating constraints");
+    1475             : 
+    1476         774 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+    1477         387 :   res.success = true;
+    1478         387 :   res.message = "constraints updated";
+    1479             : 
+    1480         387 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+    1481             : }
+    1482             : 
+    1483             : //}
+    1484             : 
+    1485             : /* //{ setReference() */
+    1486             : 
+    1487         269 : const mrs_msgs::ReferenceSrvResponse::ConstPtr MpcTracker::setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr& cmd) {
+    1488             : 
+    1489         269 :   toggleHover(false);
+    1490             : 
+    1491         269 :   setGoal(cmd->reference.position.x, cmd->reference.position.y, cmd->reference.position.z, cmd->reference.heading, true);
+    1492             : 
+    1493         538 :   mrs_msgs::ReferenceSrvResponse res;
+    1494         269 :   res.success = true;
+    1495         269 :   res.message = "reference set";
+    1496             : 
+    1497         538 :   return mrs_msgs::ReferenceSrvResponse::ConstPtr(new mrs_msgs::ReferenceSrvResponse(res));
+    1498             : }
+    1499             : 
+    1500             : //}
+    1501             : 
+    1502             : /* //{ setVelocityReference() */
+    1503             : 
+    1504         809 : const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr MpcTracker::setVelocityReference(const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr& cmd) {
+    1505             : 
+    1506         809 :   if (!is_initialized_) {
+    1507           0 :     return mrs_msgs::VelocityReferenceSrvResponse::Ptr();
+    1508             :   }
+    1509             : 
+    1510             :   {
+    1511         809 :     std::scoped_lock lock(mutex_velocity_reference_);
+    1512             : 
+    1513         809 :     velocity_reference_time_ = ros::Time::now();
+    1514             : 
+    1515         809 :     velocity_reference_ = cmd->reference;
+    1516             :   }
+    1517             : 
+    1518         809 :   if (!velocity_tracking_active_) {
+    1519             : 
+    1520           4 :     ROS_INFO("[MpcTracker]: starting velocity tracking timer");
+    1521             : 
+    1522           4 :     timer_velocity_tracking_.stop();
+    1523           4 :     timer_velocity_tracking_.start();
+    1524             : 
+    1525           4 :     velocity_tracking_active_ = true;
+    1526             :   }
+    1527             : 
+    1528        1618 :   mrs_msgs::VelocityReferenceSrvResponse response;
+    1529         809 :   response.success = true;
+    1530         809 :   response.message = "reference set";
+    1531             : 
+    1532         809 :   return mrs_msgs::VelocityReferenceSrvResponse::ConstPtr(new mrs_msgs::VelocityReferenceSrvResponse(response));
+    1533             : }
+    1534             : 
+    1535             : //}
+    1536             : 
+    1537             : /* //{ setTrajectoryReference() */
+    1538             : 
+    1539           6 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr MpcTracker::setTrajectoryReference([
+    1540             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr& cmd) {
+    1541             : 
+    1542          12 :   std::stringstream ss;
+    1543             : 
+    1544          18 :   auto [success, message, modified] = loadTrajectory(cmd->trajectory);
+    1545             : 
+    1546          12 :   mrs_msgs::TrajectoryReferenceSrvResponse response;
+    1547           6 :   response.success  = success;
+    1548           6 :   response.message  = message;
+    1549           6 :   response.modified = modified;
+    1550             : 
+    1551          12 :   return mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr(new mrs_msgs::TrajectoryReferenceSrvResponse(response));
+    1552             : }
+    1553             : 
+    1554             : //}
+    1555             : 
+    1556             : // | ------------------------ callbacks ----------------------- |
+    1557             : 
+    1558             : /* //{ callbackOtherMavTrajectory() */
+    1559             : 
+    1560         348 : void MpcTracker::callbackOtherMavTrajectory(const mrs_msgs::FutureTrajectory::ConstPtr msg) {
+    1561             : 
+    1562         348 :   if (!is_initialized_) {
+    1563           0 :     return;
+    1564             :   }
+    1565             : 
+    1566         696 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("callbackOtherMavTrajectory");
+    1567             :   mrs_lib::ScopeTimer timer =
+    1568         696 :       mrs_lib::ScopeTimer("MpcTracker::callbackOtherMavTrajectory", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1569             : 
+    1570         348 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1571             : 
+    1572         348 :   mrs_msgs::FutureTrajectory trajectory = *msg;
+    1573             : 
+    1574             :   // the times might not be synchronized, so just remember the time of receiving it
+    1575         348 :   trajectory.stamp = ros::Time::now();
+    1576             : 
+    1577             :   // transform it from the utm origin to the currently used frame
+    1578         696 :   auto res = common_handlers_->transformer->getTransform("utm_origin", uav_state.header.frame_id, ros::Time::now());
+    1579             : 
+    1580         348 :   if (!res) {
+    1581             : 
+    1582           0 :     std::string message = "[MpcTracker]: can not transform other drone trajectory to the current frame";
+    1583           0 :     ROS_WARN_STREAM_ONCE(message);
+    1584           0 :     ROS_DEBUG_STREAM_THROTTLE(1.0, message);
+    1585             : 
+    1586           0 :     return;
+    1587             :   }
+    1588             : 
+    1589         348 :   geometry_msgs::TransformStamped tf = res.value();
+    1590             : 
+    1591       14268 :   for (int i = 0; i < int(trajectory.points.size()); i++) {
+    1592             : 
+    1593       13920 :     geometry_msgs::PoseStamped original_pose;
+    1594             : 
+    1595       13920 :     original_pose.pose.position.x = trajectory.points.at(i).x;
+    1596       13920 :     original_pose.pose.position.y = trajectory.points.at(i).y;
+    1597       13920 :     original_pose.pose.position.z = trajectory.points.at(i).z;
+    1598             : 
+    1599       13920 :     original_pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    1600             : 
+    1601       13920 :     auto res = common_handlers_->transformer->transform(original_pose, tf);
+    1602             : 
+    1603       13920 :     if (res) {
+    1604       13920 :       trajectory.points.at(i).x = res.value().pose.position.x;
+    1605       13920 :       trajectory.points.at(i).y = res.value().pose.position.y;
+    1606       13920 :       trajectory.points.at(i).z = res.value().pose.position.z;
+    1607             :     } else {
+    1608             : 
+    1609           0 :       std::string message = "[MpcTracker]: could not transform point of other uav future trajectory!";
+    1610           0 :       ROS_WARN_STREAM_ONCE(message);
+    1611           0 :       ROS_DEBUG_STREAM_THROTTLE(1.0, message);
+    1612             : 
+    1613           0 :       return;
+    1614             :     }
+    1615             :   }
+    1616             : 
+    1617             :   {
+    1618         696 :     std::scoped_lock lock(mutex_other_uav_avoidance_trajectories_);
+    1619             : 
+    1620             :     // update the diagnostics
+    1621         348 :     other_uav_avoidance_trajectories_[trajectory.uav_name] = trajectory;
+    1622             :   }
+    1623             : }
+    1624             : 
+    1625             : //}
+    1626             : 
+    1627             : /* //{ callbackOtherMavDiagnostics() */
+    1628             : 
+    1629        2189 : void MpcTracker::callbackOtherMavDiagnostics(const mrs_msgs::MpcTrackerDiagnostics::ConstPtr msg) {
+    1630             : 
+    1631        6567 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("callbackOtherMavDiagnostics");
+    1632             :   mrs_lib::ScopeTimer timer =
+    1633        6567 :       mrs_lib::ScopeTimer("MpcTracker::callbackOtherMavDiagnostics", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1634             : 
+    1635        4378 :   mrs_msgs::MpcTrackerDiagnostics diagnostics = *msg;
+    1636             : 
+    1637             :   // fill in the current time
+    1638             :   // the other uav's time might not be synchronized with ours
+    1639        2189 :   diagnostics.header.stamp = ros::Time::now();
+    1640             : 
+    1641             :   {
+    1642        4378 :     std::scoped_lock lock(mutex_other_uav_diagnostics_);
+    1643             : 
+    1644        2189 :     other_uav_diagnostics_[diagnostics.uav_name] = diagnostics;
+    1645             :   }
+    1646        2189 : }
+    1647             : 
+    1648             : //}
+    1649             : 
+    1650             : /* //{ callbackToggleCollisionAvoidance() */
+    1651             : 
+    1652           0 : bool MpcTracker::callbackToggleCollisionAvoidance(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    1653             : 
+    1654           0 :   collision_avoidance_enabled_ = req.data;
+    1655             : 
+    1656           0 :   ROS_INFO("[MpcTracker]: Collision avoidance was switched %s", collision_avoidance_enabled_ ? "TRUE" : "FALSE");
+    1657             : 
+    1658           0 :   res.message = "Collision avoidance set.";
+    1659           0 :   res.success = true;
+    1660             : 
+    1661           0 :   return true;
+    1662             : }
+    1663             : 
+    1664             : //}
+    1665             : 
+    1666             : /* callbackWiggle() //{ */
+    1667             : 
+    1668           0 : bool MpcTracker::callbackWiggle(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    1669             : 
+    1670           0 :   if (!is_initialized_) {
+    1671             : 
+    1672           0 :     res.success = false;
+    1673           0 :     res.message = "tracker not active";
+    1674           0 :     return true;
+    1675             :   }
+    1676             : 
+    1677             :   {
+    1678           0 :     std::scoped_lock lock(mutex_drs_params_);
+    1679             : 
+    1680           0 :     drs_params_.wiggle_enabled = req.data;
+    1681             : 
+    1682           0 :     reconfigure_server_->updateConfig(drs_params_);
+    1683             :   }
+    1684             : 
+    1685           0 :   res.success = true;
+    1686           0 :   res.message = "wiggle updated";
+    1687             : 
+    1688           0 :   return true;
+    1689             : }
+    1690             : 
+    1691             : //}
+    1692             : 
+    1693             : /* //{ dynamicReconfigureCallback() */
+    1694             : 
+    1695         109 : void MpcTracker::dynamicReconfigureCallback(mrs_uav_trackers::mpc_trackerConfig& config, [[maybe_unused]] uint32_t level) {
+    1696             : 
+    1697         218 :   std::scoped_lock lock(mutex_drs_params_);
+    1698             : 
+    1699         109 :   drs_params_ = config;
+    1700             : 
+    1701         109 :   ROS_INFO("[MpcTracker]: DRS updated");
+    1702         109 : }
+    1703             : 
+    1704             : //}
+    1705             : 
+    1706             : // --------------------------------------------------------------
+    1707             : // |                          routines                          |
+    1708             : // --------------------------------------------------------------
+    1709             : 
+    1710             : // | --------------- mutual collision avoidance --------------- |
+    1711             : 
+    1712             : /* //{ checkCollision() */
+    1713             : 
+    1714      187680 : bool MpcTracker::checkCollision(const double ax, const double ay, const double az, const double bx, const double by, const double bz) {
+    1715             : 
+    1716      187680 :   if (mrs_lib::geometry::dist(vec2_t(ax, ay), vec2_t(bx, by)) < _avoidance_radius_threshold_ && fabs(az - bz) < _avoidance_z_threshold_) {
+    1717             : 
+    1718       11876 :     return true;
+    1719             : 
+    1720             :   } else {
+    1721             : 
+    1722      175804 :     return false;
+    1723             :   }
+    1724             : }
+    1725             : 
+    1726             : //}
+    1727             : 
+    1728             : /* //{ checkCollisionInflated() */
+    1729             : 
+    1730      187680 : bool MpcTracker::checkCollisionInflated(const double ax, const double ay, const double az, const double bx, const double by, const double bz) {
+    1731             : 
+    1732      187680 :   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) {
+    1733             : 
+    1734       67294 :     return true;
+    1735             : 
+    1736             :   } else {
+    1737             : 
+    1738      120386 :     return false;
+    1739             :   }
+    1740             : }
+    1741             : 
+    1742             : //}
+    1743             : 
+    1744             : /* //{ checkTrajectoryForCollisions() */
+    1745             : 
+    1746             : // Check for potential collisions and return the needed altitude offset to avoid other drones
+    1747       83422 : double MpcTracker::checkTrajectoryForCollisions(int& first_collision_index) {
+    1748             : 
+    1749       83422 :   std::scoped_lock lock(mutex_predicted_trajectory_, mutex_des_trajectory_, mutex_other_uav_avoidance_trajectories_);
+    1750             : 
+    1751       83422 :   first_collision_index = INT_MAX;
+    1752       83422 :   avoiding_collision_   = false;
+    1753             : 
+    1754       83422 :   std::map<std::string, mrs_msgs::FutureTrajectory>::iterator u = other_uav_avoidance_trajectories_.begin();
+    1755             : 
+    1756       88114 :   while (u != other_uav_avoidance_trajectories_.end()) {
+    1757             : 
+    1758             :     // is the other's trajectory fresh enought?
+    1759        4692 :     if ((ros::Time::now() - u->second.stamp).toSec() < _collision_trajectory_timeout_) {
+    1760             : 
+    1761      192372 :       for (int v = 0; v < MPC_HORIZON_LENGTH; v++) {
+    1762             : 
+    1763             :         // check all points of the trajectory for possible collisions
+    1764      187680 :         if (checkCollision(predicted_trajectory_(v * MPC_N_STATES, 0), predicted_trajectory_(v * MPC_N_STATES + 4, 0),
+    1765      187680 :                            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)) {
+    1766             : 
+    1767             :           // collision is detected
+    1768       11876 :           int other_uav_priority = INT_MAX;
+    1769             :           // get the priority of the other uav
+    1770             :           /* sscanf(u->first.c_str(), "uav%d", &other_uav_priority); */
+    1771       11876 :           other_uav_priority = u->second.priority;
+    1772             : 
+    1773             :           // check if we should be avoiding (out priority is higher, or the other uav has collision avoidance turned off)
+    1774       11876 :           if ((u->second.collision_avoidance == false) || (other_uav_priority < avoidance_this_uav_priority_)) {
+    1775             : 
+    1776             :             // we should be avoiding
+    1777        6339 :             avoiding_collision_      = true;
+    1778        6339 :             double tmp_safe_altitude = u->second.points.at(v).z + _avoidance_z_correction_;
+    1779             : 
+    1780        6339 :             if (tmp_safe_altitude > collision_free_altitude_ && v <= _avoidance_collision_start_climbing_) {
+    1781         372 :               collision_free_altitude_ = tmp_safe_altitude;
+    1782             :             }
+    1783             : 
+    1784        6339 :             ROS_ERROR_STREAM_THROTTLE(1, "[MpcTracker]: avoiding collision with uav" << other_uav_priority);
+    1785             : 
+    1786             :           } else {
+    1787             :             // the other uav should avoid us
+    1788        5537 :             ROS_WARN_STREAM_THROTTLE(1, "[MpcTracker]: detected collision with uav" << other_uav_priority << ", not avoiding (my priority is higher)");
+    1789             :           }
+    1790             :         }
+    1791             : 
+    1792      187680 :         if (checkCollisionInflated(predicted_trajectory_(v * MPC_N_STATES, 0), predicted_trajectory_(v * MPC_N_STATES + 4, 0),
+    1793      187680 :                                    predicted_trajectory_(v * MPC_N_STATES + 8, 0), u->second.points.at(v).x, u->second.points.at(v).y,
+    1794      187680 :                                    u->second.points.at(v).z)) {
+    1795             : 
+    1796             :           // collision is detected
+    1797       67294 :           if (first_collision_index > v) {
+    1798        2522 :             first_collision_index = v;
+    1799             :           }
+    1800             :         }
+    1801             :       }
+    1802             :     }
+    1803             : 
+    1804        4692 :     u++;
+    1805             :   }
+    1806             : 
+    1807       83422 :   if (!avoiding_collision_) {
+    1808             : 
+    1809       82778 :     auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    1810             : 
+    1811             :     // we are not avoiding any collisions, so we slowly reduce the collision avoidance offset to return to normal flight
+    1812       82778 :     collision_free_altitude_ -= 2.0 / (1.0 / dt1);
+    1813             : 
+    1814       82778 :     double safety_area_min_z = std::numeric_limits<double>::lowest();
+    1815             : 
+    1816       82778 :     if (collision_free_altitude_ < safety_area_min_z) {
+    1817             : 
+    1818           0 :       collision_free_altitude_ = safety_area_min_z;
+    1819             :     }
+    1820             :   }
+    1821             : 
+    1822      166844 :   return collision_free_altitude_;
+    1823             : }
+    1824             : 
+    1825             : //}
+    1826             : 
+    1827             : // | ------------------ trajectory filtering ------------------ |
+    1828             : 
+    1829             : /* //{ filterReferenceXY() */
+    1830             : 
+    1831       84480 : std::tuple<MatrixXd, MatrixXd> MpcTracker::filterReferenceXY(const VectorXd& des_x_trajectory, const VectorXd& des_y_trajectory, double max_speed_x,
+    1832             :                                                              double max_speed_y) {
+    1833             : 
+    1834       84480 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    1835             : 
+    1836      168960 :   auto mpc_x         = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    1837       84480 :   auto trajectory_dt = mrs_lib::get_mutexed(mutex_des_trajectory_, trajectory_dt_);
+    1838             : 
+    1839      168960 :   MatrixXd filtered_x_trajectory = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+    1840      168960 :   MatrixXd filtered_y_trajectory = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+    1841             : 
+    1842             :   double difference_x;
+    1843             :   double difference_y;
+    1844             :   double max_sample_x;
+    1845             :   double max_sample_y;
+    1846             : 
+    1847       84480 :   if (std::hypot(mpc_x(0, 0) - des_x_trajectory(0, 0), mpc_x(4, 0) - des_y_trajectory(0, 0)) < 2.0) {
+    1848       62437 :     return {des_x_trajectory, des_y_trajectory};
+    1849             :   }
+    1850             : 
+    1851      903763 :   for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    1852             : 
+    1853      881720 :     if (i == 0) {
+    1854       22043 :       max_sample_x = max_speed_x * dt1;
+    1855       22043 :       max_sample_y = max_speed_y * dt1;
+    1856       22043 :       difference_x = des_x_trajectory(i, 0) - mpc_x(0, 0);
+    1857       22043 :       difference_y = des_y_trajectory(i, 0) - mpc_x(4, 0);
+    1858             :     } else {
+    1859      859677 :       max_sample_x = max_speed_x * _dt2_;
+    1860      859677 :       max_sample_y = max_speed_y * _dt2_;
+    1861      859677 :       difference_x = des_x_trajectory(i, 0) - filtered_x_trajectory(i - 1, 0);
+    1862      859677 :       difference_y = des_y_trajectory(i, 0) - filtered_y_trajectory(i - 1, 0);
+    1863             :     }
+    1864             : 
+    1865      881720 :     if (!trajectory_tracking_in_progress_) {
+    1866             : 
+    1867      808160 :       double direction_angle  = atan2(difference_y, difference_x);
+    1868      808160 :       double max_dir_sample_x = abs(max_sample_x * cos(direction_angle));
+    1869      808160 :       double max_dir_sample_y = abs(max_sample_y * sin(direction_angle));
+    1870             : 
+    1871      808160 :       if (max_sample_x > max_dir_sample_x) {
+    1872      239577 :         max_sample_x = max_dir_sample_x;
+    1873             :       }
+    1874      808160 :       if (max_sample_y > max_dir_sample_y) {
+    1875      808160 :         max_sample_y = max_dir_sample_y;
+    1876             :       }
+    1877             : 
+    1878             :       // saturate the difference
+    1879      808160 :       if (difference_x > max_sample_x)
+    1880      159547 :         difference_x = max_sample_x;
+    1881      648613 :       else if (difference_x < -max_sample_x)
+    1882      162220 :         difference_x = -max_sample_x;
+    1883             : 
+    1884      808160 :       if (difference_y > max_sample_y)
+    1885       89744 :         difference_y = max_sample_y;
+    1886      718416 :       else if (difference_y < -max_sample_y)
+    1887      167482 :         difference_y = -max_sample_y;
+    1888             :     }
+    1889             : 
+    1890      881720 :     if (i == 0) {
+    1891       22043 :       filtered_x_trajectory(i, 0) = mpc_x(0, 0) + difference_x;
+    1892       22043 :       filtered_y_trajectory(i, 0) = mpc_x(4, 0) + difference_y;
+    1893             :     } else {
+    1894      859677 :       filtered_x_trajectory(i, 0) = filtered_x_trajectory(i - 1, 0) + difference_x;
+    1895      859677 :       filtered_y_trajectory(i, 0) = filtered_y_trajectory(i - 1, 0) + difference_y;
+    1896             :     }
+    1897             :   }
+    1898             : 
+    1899             :   // | ----------------------- add wiggle ----------------------- |
+    1900             : 
+    1901       22043 :   auto [wiggle_enabled, wiggle_amplitude, wiggle_frequency_] =
+    1902       22043 :       mrs_lib::get_mutexed(mutex_drs_params_, drs_params_.wiggle_enabled, drs_params_.wiggle_amplitude, drs_params_.wiggle_frequency);
+    1903             : 
+    1904       22043 :   if (wiggle_enabled) {
+    1905             : 
+    1906           0 :     for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    1907           0 :       filtered_x_trajectory(i, 0) += wiggle_amplitude * cos(wiggle_frequency_ * 2 * M_PI * i * trajectory_dt + wiggle_phase_);
+    1908           0 :       filtered_y_trajectory(i, 0) += wiggle_amplitude * sin(wiggle_frequency_ * 2 * M_PI * i * trajectory_dt + wiggle_phase_);
+    1909             :     }
+    1910             : 
+    1911           0 :     wiggle_phase_ += wiggle_frequency_ * dt1 * 2 * M_PI;
+    1912             : 
+    1913           0 :     if (wiggle_phase_ > M_PI) {
+    1914           0 :       wiggle_phase_ -= 2 * M_PI;
+    1915             :     }
+    1916             :   }
+    1917             : 
+    1918       22043 :   return std::make_tuple(filtered_x_trajectory, filtered_y_trajectory);
+    1919             : }
+    1920             : 
+    1921             : //}
+    1922             : 
+    1923             : /* //{ filterReferenceZ() */
+    1924             : 
+    1925       84480 : MatrixXd MpcTracker::filterReferenceZ(const VectorXd& des_z_trajectory, const double max_ascending_speed, const double max_descending_speed) {
+    1926             : 
+    1927      168960 :   auto mpc_x = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    1928             : 
+    1929       84480 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    1930             : 
+    1931             :   double difference_z;
+    1932             :   double max_sample_z;
+    1933             : 
+    1934       84480 :   MatrixXd filtered_trajectory = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+    1935             : 
+    1936       84480 :   double current_z = mpc_x(8, 0);
+    1937             : 
+    1938     3463676 :   for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    1939             : 
+    1940     3379200 :     if (i == 0) {
+    1941             : 
+    1942       84480 :       difference_z = des_z_trajectory(i, 0) - current_z;
+    1943             : 
+    1944       84480 :       if (difference_z > 0) {
+    1945       35544 :         max_sample_z = max_ascending_speed * dt1;
+    1946             :       } else {
+    1947       48936 :         max_sample_z = max_descending_speed * dt1;
+    1948             :       }
+    1949             : 
+    1950             :     } else {
+    1951             : 
+    1952     3294724 :       difference_z = des_z_trajectory(i, 0) - filtered_trajectory(i - 1, 0);
+    1953             : 
+    1954     3294724 :       if (difference_z > 0) {
+    1955      213746 :         max_sample_z = max_ascending_speed * _dt2_;
+    1956             :       } else {
+    1957     3080970 :         max_sample_z = max_descending_speed * _dt2_;
+    1958             :       }
+    1959             :     }
+    1960             : 
+    1961     3379200 :     if (!trajectory_tracking_in_progress_) {
+    1962             : 
+    1963             :       // saturate the difference
+    1964     2748080 :       if (difference_z > max_sample_z)
+    1965       83099 :         difference_z = max_sample_z;
+    1966     2664977 :       else if (difference_z < -max_sample_z)
+    1967       22116 :         difference_z = -max_sample_z;
+    1968             :     }
+    1969             : 
+    1970     3379200 :     if (i == 0) {
+    1971       84480 :       filtered_trajectory(i, 0) = current_z + difference_z;
+    1972             :     } else {
+    1973     3294724 :       filtered_trajectory(i, 0) = filtered_trajectory(i - 1, 0) + difference_z;
+    1974             :     }
+    1975             :   }
+    1976             : 
+    1977      168960 :   return filtered_trajectory;
+    1978             : }
+    1979             : 
+    1980             : //}
+    1981             : 
+    1982             : /* //{ manageConstraints() */
+    1983             : 
+    1984       84480 : void MpcTracker::manageConstraints() {
+    1985             : 
+    1986       84480 :   if (!got_constraints_) {
+    1987       84300 :     return;
+    1988             :   }
+    1989             : 
+    1990       84480 :   if (all_constraints_set_) {
+    1991       84300 :     return;
+    1992             :   }
+    1993             : 
+    1994         180 :   auto constraints            = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    1995         360 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    1996             : 
+    1997         360 :   bool can_change = (fabs(mpc_x(1, 0)) < constraints.horizontal_speed) && (fabs(mpc_x(2, 0)) < constraints.horizontal_acceleration) &&
+    1998         180 :                     (fabs(mpc_x(3, 0)) < constraints.horizontal_jerk) && (fabs(mpc_x(5, 0)) < constraints.horizontal_speed) &&
+    1999         180 :                     (fabs(mpc_x(6, 0)) < constraints.horizontal_acceleration) && (fabs(mpc_x(7, 0)) < constraints.horizontal_jerk) &&
+    2000         180 :                     (mpc_x(9, 0) < constraints.vertical_ascending_speed) && (mpc_x(9, 0) > -constraints.vertical_descending_speed) &&
+    2001         180 :                     (mpc_x(10, 0) < constraints.vertical_ascending_acceleration) && (mpc_x(10, 0) > -constraints.vertical_descending_acceleration) &&
+    2002         180 :                     (mpc_x(11, 0) < constraints.vertical_ascending_jerk) && (mpc_x(11, 0) > -constraints.vertical_descending_jerk) &&
+    2003         540 :                     (fabs(mpc_x_heading(1, 0)) < constraints.heading_speed) && (fabs(mpc_x_heading(2, 0)) < constraints.heading_acceleration) &&
+    2004         180 :                     (fabs(mpc_x_heading(3, 0)) < constraints.heading_jerk);
+    2005             : 
+    2006         180 :   if (can_change) {
+    2007             : 
+    2008             :     {
+    2009         180 :       std::scoped_lock lock(mutex_constraints_filtered_);
+    2010             : 
+    2011         180 :       constraints_filtered_.horizontal_acceleration = constraints.horizontal_acceleration;
+    2012         180 :       constraints_filtered_.horizontal_jerk         = constraints.horizontal_jerk;
+    2013         180 :       constraints_filtered_.horizontal_snap         = constraints.horizontal_snap;
+    2014             : 
+    2015         180 :       constraints_filtered_.vertical_ascending_acceleration = constraints.vertical_ascending_acceleration;
+    2016         180 :       constraints_filtered_.vertical_ascending_jerk         = constraints.vertical_ascending_jerk;
+    2017         180 :       constraints_filtered_.vertical_ascending_snap         = constraints.vertical_ascending_snap;
+    2018             : 
+    2019         180 :       constraints_filtered_.vertical_descending_acceleration = constraints.vertical_descending_acceleration;
+    2020         180 :       constraints_filtered_.vertical_descending_jerk         = constraints.vertical_descending_jerk;
+    2021         180 :       constraints_filtered_.vertical_descending_snap         = constraints.vertical_descending_snap;
+    2022             : 
+    2023         180 :       constraints_filtered_.heading_acceleration = constraints.heading_acceleration;
+    2024         180 :       constraints_filtered_.heading_jerk         = constraints.heading_jerk;
+    2025         180 :       constraints_filtered_.heading_snap         = constraints.heading_snap;
+    2026             :     }
+    2027             : 
+    2028         180 :     ROS_INFO_THROTTLE(1.0, "[MpcTracker]: all constraints succesfully applied");
+    2029         180 :     all_constraints_set_ = true;
+    2030             : 
+    2031             :   } else {
+    2032           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: slowing down to apply new constraints");
+    2033             :   }
+    2034             : }
+    2035             : 
+    2036             : //}
+    2037             : 
+    2038             : /* //{ calculateMPC() */
+    2039             : 
+    2040       84480 : void MpcTracker::calculateMPC() {
+    2041             : 
+    2042       84480 :   std::scoped_lock lock(mutex_mpc_calculation_);
+    2043             : 
+    2044       84480 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    2045             : 
+    2046       84480 :   ROS_DEBUG_STREAM_THROTTLE(1.0, "[MpcTracker]: MPC calculation dt = " << dt1);
+    2047             : 
+    2048       84480 :   auto constraints            = mrs_lib::get_mutexed(mutex_constraints_filtered_, constraints_filtered_);
+    2049       84480 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    2050       84480 :   auto uav_state              = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    2051       84480 :   auto drs_params             = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+    2052             : 
+    2053       84480 :   MatrixXd des_x_trajectory, des_y_trajectory, des_z_trajectory, des_heading_trajectory;
+    2054             :   {
+    2055      168960 :     std::scoped_lock lock(mutex_des_trajectory_);
+    2056             : 
+    2057       84480 :     des_x_trajectory       = des_x_trajectory_;
+    2058       84480 :     des_y_trajectory       = des_y_trajectory_;
+    2059       84480 :     des_z_trajectory       = des_z_trajectory_;
+    2060       84480 :     des_heading_trajectory = des_heading_trajectory_;
+    2061             :   }
+    2062             : 
+    2063       84480 :   int    first_collision_index = INT_MAX;
+    2064       84480 :   double lowest_z              = std::numeric_limits<double>::max();
+    2065             : 
+    2066       84480 :   if (collision_avoidance_enabled_ && (uav_state.estimator_horizontal == "lat_gps" || uav_state.estimator_horizontal == "lat_rtk")) {
+    2067             : 
+    2068             :     // determine the lowest point in our trajectory
+    2069     3420303 :     for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    2070     3336880 :       if (des_z_trajectory_(i, 0) < lowest_z) {
+    2071      122814 :         lowest_z = des_z_trajectory_(i, 0);
+    2072             :       }
+    2073             :     }
+    2074             : 
+    2075             :     // check other drone trajectories for collisions
+    2076       83422 :     minimum_collison_free_altitude_ = checkTrajectoryForCollisions(first_collision_index);
+    2077             : 
+    2078             :   } else {
+    2079             : 
+    2080        1058 :     minimum_collison_free_altitude_ = std::numeric_limits<double>::lowest();
+    2081             :   }
+    2082             : 
+    2083       84480 :   double max_speed_x = constraints.horizontal_speed;
+    2084       84480 :   double max_speed_y = constraints.horizontal_speed;
+    2085       84480 :   double max_speed_z = constraints.vertical_ascending_speed;
+    2086       84480 :   double min_speed_z = constraints.vertical_descending_speed;
+    2087             : 
+    2088       84480 :   double max_acc_x = constraints.horizontal_acceleration;
+    2089       84480 :   double max_acc_y = constraints.horizontal_acceleration;
+    2090       84480 :   double max_acc_z = constraints.vertical_ascending_acceleration;
+    2091       84480 :   double min_acc_z = constraints.vertical_descending_acceleration;
+    2092             : 
+    2093       84480 :   double max_snap_x = constraints.horizontal_snap;
+    2094       84480 :   double max_snap_y = constraints.horizontal_snap;
+    2095       84480 :   double max_snap_z = constraints.vertical_ascending_snap;
+    2096       84480 :   double min_snap_z = constraints.vertical_descending_snap;
+    2097             : 
+    2098       84480 :   double max_jerk_x = constraints.horizontal_jerk;
+    2099       84480 :   double max_jerk_y = constraints.horizontal_jerk;
+    2100       84480 :   double max_jerk_z = constraints.vertical_ascending_jerk;
+    2101       84480 :   double min_jerk_z = constraints.vertical_descending_jerk;
+    2102             : 
+    2103       84480 :   collision_avoidance_affecting_me_ = false;
+    2104             : 
+    2105       84480 :   if (first_collision_index < MPC_HORIZON_LENGTH) {
+    2106             : 
+    2107        2522 :     collision_avoidance_affecting_me_ = true;
+    2108             :     // the tmp variable is used to scale the speed of our drone in collision avoidance, depending on how far away the collision is
+    2109        2522 :     double tmp = 0;
+    2110             : 
+    2111        2522 :     if (first_collision_index <= _avoidance_collision_slow_down_fully_) {
+    2112        2522 :       tmp = 1;
+    2113           0 :     } else if (first_collision_index <= _avoidance_collision_slow_down_) {
+    2114           0 :       tmp = 1.0 - ((double)(first_collision_index - _avoidance_collision_slow_down_fully_)) /
+    2115           0 :                       (double)(_avoidance_collision_slow_down_ - _avoidance_collision_slow_down_fully_);
+    2116           0 :       tmp = tmp * tmp;
+    2117             :     }
+    2118             : 
+    2119        2522 :     if (!std::isfinite(tmp)) {
+    2120           0 :       tmp = 1.0;
+    2121           0 :       ROS_ERROR("[MpcTracker]: NaN detected in variable 'tmp', setting it to 1.0 and returning!!!");
+    2122           0 :       return;
+    2123        2522 :     } else if (tmp > 1.0) {
+    2124           0 :       tmp = 1.0;
+    2125        2522 :     } else if (tmp < 0.0) {
+    2126           0 :       tmp = 0.0;
+    2127             :     }
+    2128             : 
+    2129        2522 :     if (tmp > coef_scaler) {
+    2130           6 :       coef_scaler = tmp;
+    2131           6 :       coef_time   = ros::Time::now();
+    2132             :     }
+    2133        2522 :     if ((ros::Time::now() - coef_time).toSec() > 2.0) {
+    2134        2065 :       coef_scaler = tmp;
+    2135             :     }
+    2136             : 
+    2137             :     // we are close to a possible collision, better slow down a bit to give everyone more time
+    2138        2522 :     max_speed_x = constraints.horizontal_speed * ((_avoidance_collision_horizontal_speed_coef_ * coef_scaler) + (1.0 - coef_scaler));
+    2139        2522 :     max_speed_y = constraints.horizontal_speed * ((_avoidance_collision_horizontal_speed_coef_ * coef_scaler) + (1.0 - coef_scaler));
+    2140             :   }
+    2141             : 
+    2142       84480 :   if (collision_free_altitude_ > lowest_z) {
+    2143             : 
+    2144        1487 :     collision_avoidance_affecting_me_ = true;
+    2145        1487 :     max_speed_x                       = constraints.horizontal_speed * (_avoidance_collision_horizontal_speed_coef_);
+    2146        1487 :     max_speed_y                       = constraints.horizontal_speed * (_avoidance_collision_horizontal_speed_coef_);
+    2147             :   }
+    2148             : 
+    2149             :   // first control input generated by MPC
+    2150      168960 :   VectorXd mpc_u         = VectorXd::Zero(MPC_N_INPUTS);
+    2151       84480 :   double   mpc_u_heading = 0;
+    2152             : 
+    2153       84480 :   double iters_z       = 0;
+    2154       84480 :   double iters_x       = 0;
+    2155       84480 :   double iters_y       = 0;
+    2156       84480 :   double iters_heading = 0;
+    2157             : 
+    2158       84480 :   ros::Time time_begin = ros::Time::now();
+    2159             : 
+    2160      168960 :   MatrixXd des_z_filtered = filterReferenceZ(des_z_trajectory, max_speed_z, min_speed_z);
+    2161             : 
+    2162     3463676 :   for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    2163     3379200 :     if (des_z_filtered(i, 0) < minimum_collison_free_altitude_) {
+    2164       58417 :       des_z_filtered_offset_(i, 0) = minimum_collison_free_altitude_;
+    2165             :     } else {
+    2166     3320783 :       des_z_filtered_offset_(i, 0) = des_z_filtered(i, 0);
+    2167             :     }
+    2168             :   }
+    2169             : 
+    2170             :   // | ----------------- prepare the references ----------------- |
+    2171             : 
+    2172             :   // | -------------------- MPC solver z-axis ------------------- |
+    2173             : 
+    2174       84480 :   if (brake_ && !trajectory_tracking_in_progress_) {
+    2175       64885 :     mpc_solver_z_->setVelQ(drs_params.q_vel_braking);
+    2176             :   } else {
+    2177       19595 :     mpc_solver_z_->setVelQ(drs_params.q_vel_no_braking);
+    2178             :   }
+    2179             : 
+    2180      168960 :   MatrixXd initial_z = MatrixXd::Zero(4, 1);
+    2181             : 
+    2182       84480 :   initial_z(0, 0) = mpc_x(8, 0);
+    2183       84480 :   initial_z(1, 0) = mpc_x(9, 0);
+    2184       84480 :   initial_z(2, 0) = mpc_x(10, 0);
+    2185       84480 :   initial_z(3, 0) = mpc_x(11, 0);
+    2186             : 
+    2187       84480 :   mpc_solver_z_->setDt(dt1);
+    2188       84480 :   mpc_solver_z_->setInitialState(initial_z);
+    2189       84480 :   mpc_solver_z_->loadReference(des_z_filtered_offset_);
+    2190       84480 :   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);
+    2191       84480 :   iters_z += mpc_solver_z_->solveMPC();
+    2192             : 
+    2193             :   {
+    2194      168960 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2195             : 
+    2196       84480 :     mpc_solver_z_->getStates(predicted_trajectory_);
+    2197             :   }
+    2198             : 
+    2199       84480 :   mpc_u(2) = mpc_solver_z_->getFirstControlInput();
+    2200             : 
+    2201             :   // if we are climbing to avoid a collision, reduce or arrest our horizontal velocity
+    2202             :   double ascend;
+    2203             :   {
+    2204       84480 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2205             : 
+    2206       84480 :     ascend = (predicted_trajectory_(10, 0) / max_speed_z);
+    2207             :   }
+    2208             : 
+    2209       84480 :   if (ascend > 0 && collision_free_altitude_ > lowest_z) {
+    2210         601 :     max_speed_y = max_speed_y * (1.0 - ascend);
+    2211         601 :     max_speed_x = max_speed_x * (1.0 - ascend);
+    2212             :   }
+    2213             : 
+    2214      253440 :   auto [des_x_filtered, des_y_filtered] = filterReferenceXY(des_x_trajectory, des_y_trajectory, max_speed_x, max_speed_y);
+    2215             : 
+    2216             :   // | -------------------- MPC solver x-axis ------------------- |
+    2217             : 
+    2218       84480 :   if (brake_ && !trajectory_tracking_in_progress_) {
+    2219       64885 :     mpc_solver_x_->setVelQ(drs_params.q_vel_braking);
+    2220             :   } else {
+    2221       19595 :     mpc_solver_x_->setVelQ(drs_params.q_vel_no_braking);
+    2222             :   }
+    2223             : 
+    2224      168960 :   MatrixXd initial_x = MatrixXd::Zero(4, 1);
+    2225             : 
+    2226       84480 :   initial_x(0, 0) = mpc_x(0, 0);
+    2227       84480 :   initial_x(1, 0) = mpc_x(1, 0);
+    2228       84480 :   initial_x(2, 0) = mpc_x(2, 0);
+    2229       84480 :   initial_x(3, 0) = mpc_x(3, 0);
+    2230             : 
+    2231       84480 :   mpc_solver_x_->setDt(dt1);
+    2232       84480 :   mpc_solver_x_->setInitialState(initial_x);
+    2233       84480 :   mpc_solver_x_->loadReference(des_x_filtered);
+    2234             : 
+    2235       84480 :   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);
+    2236       84480 :   iters_x += mpc_solver_x_->solveMPC();
+    2237             : 
+    2238             :   {
+    2239      168960 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2240             : 
+    2241       84480 :     mpc_solver_x_->getStates(predicted_trajectory_);
+    2242             :   }
+    2243             : 
+    2244       84480 :   mpc_u(0) = mpc_solver_x_->getFirstControlInput();
+    2245             : 
+    2246             :   // | -------------------- MPC solver y-axis ------------------- |
+    2247             : 
+    2248       84480 :   if (brake_ && !trajectory_tracking_in_progress_) {
+    2249       64883 :     mpc_solver_y_->setVelQ(drs_params.q_vel_braking);
+    2250             :   } else {
+    2251       19597 :     mpc_solver_y_->setVelQ(drs_params.q_vel_no_braking);
+    2252             :   }
+    2253             : 
+    2254      168960 :   MatrixXd initial_y = MatrixXd::Zero(4, 1);
+    2255             : 
+    2256       84480 :   initial_y(0, 0) = mpc_x(4, 0);
+    2257       84480 :   initial_y(1, 0) = mpc_x(5, 0);
+    2258       84480 :   initial_y(2, 0) = mpc_x(6, 0);
+    2259       84480 :   initial_y(3, 0) = mpc_x(7, 0);
+    2260             : 
+    2261       84480 :   mpc_solver_y_->setDt(dt1);
+    2262       84480 :   mpc_solver_y_->setInitialState(initial_y);
+    2263       84480 :   mpc_solver_y_->loadReference(des_y_filtered);
+    2264       84480 :   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);
+    2265       84480 :   iters_y += mpc_solver_y_->solveMPC();
+    2266             :   {
+    2267      168960 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2268             : 
+    2269       84480 :     mpc_solver_y_->getStates(predicted_trajectory_);
+    2270             :   }
+    2271       84480 :   mpc_u(1) = mpc_solver_y_->getFirstControlInput();
+    2272             : 
+    2273             :   // | ------------------- MPC solver heading ------------------- |
+    2274             : 
+    2275             :   // unwrap the heading reference
+    2276             : 
+    2277       84480 :   des_heading_trajectory(0, 0) = sradians::unwrap(des_heading_trajectory(0, 0), mpc_x_heading(0));
+    2278             : 
+    2279     3379200 :   for (int i = 1; i < MPC_HORIZON_LENGTH; i++) {
+    2280     3294724 :     des_heading_trajectory(i, 0) = sradians::unwrap(des_heading_trajectory(i, 0), des_heading_trajectory(i - 1, 0));
+    2281             :   }
+    2282             : 
+    2283       84480 :   if (brake_ && !trajectory_tracking_in_progress_) {
+    2284       64883 :     mpc_solver_heading_->setVelQ(drs_params.q_vel_braking);
+    2285             :   } else {
+    2286       19597 :     mpc_solver_heading_->setVelQ(drs_params.q_vel_no_braking);
+    2287             :   }
+    2288             : 
+    2289       84480 :   mpc_solver_heading_->setDt(dt1);
+    2290       84480 :   mpc_solver_heading_->setInitialState(mpc_x_heading);
+    2291       84480 :   mpc_solver_heading_->loadReference(des_heading_trajectory);
+    2292       84480 :   mpc_solver_heading_->setLimits(constraints.heading_speed, constraints.heading_speed, constraints.heading_acceleration, constraints.heading_acceleration,
+    2293             :                                  constraints.heading_jerk, constraints.heading_jerk, constraints.heading_snap, constraints.heading_snap);
+    2294       84480 :   iters_heading += mpc_solver_heading_->solveMPC();
+    2295             :   {
+    2296      168960 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2297             : 
+    2298       84480 :     mpc_solver_heading_->getStates(predicted_heading_trajectory_);
+    2299             :   }
+    2300       84480 :   mpc_u_heading = mpc_solver_heading_->getFirstControlInput();
+    2301             : 
+    2302             :   {
+    2303      168960 :     geometry_msgs::PoseStamped point;
+    2304       84480 :     point.header.stamp    = ros::Time::now();
+    2305       84480 :     point.header.frame_id = uav_state_.header.frame_id;
+    2306             : 
+    2307       84480 :     point.pose.position.x = des_x_filtered(0, 0);
+    2308       84480 :     point.pose.position.y = des_y_filtered(0, 0);
+    2309       84480 :     point.pose.position.z = des_z_filtered(0, 0);
+    2310             : 
+    2311       84480 :     point.pose.orientation = mrs_lib::AttitudeConverter(0, 0, des_heading_trajectory(0, 0));
+    2312             : 
+    2313       84480 :     ph_first_reference_point_.publish(point);
+    2314             :   }
+    2315             : 
+    2316             :   {
+    2317       84480 :     bool saturating = false;
+    2318             : 
+    2319       84480 :     if (mpc_u(0) > max_snap_x * 1.01) {
+    2320           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap X: " << mpc_u(0));
+    2321           0 :       mpc_u(0)   = max_snap_x;
+    2322           0 :       saturating = true;
+    2323             :     }
+    2324       84480 :     if (mpc_u(0) < -max_snap_x * 1.01) {
+    2325           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap X: " << mpc_u(0));
+    2326           0 :       mpc_u(0)   = -max_snap_x;
+    2327           0 :       saturating = true;
+    2328             :     }
+    2329       84480 :     if (mpc_u(1) > max_snap_y * 1.01) {
+    2330           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap Y: " << mpc_u(1));
+    2331           0 :       mpc_u(1)   = max_snap_y;
+    2332           0 :       saturating = true;
+    2333             :     }
+    2334       84480 :     if (mpc_u(1) < -max_snap_y * 1.01) {
+    2335           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap Y: " << mpc_u(1));
+    2336           0 :       mpc_u(1)   = -max_snap_y;
+    2337           0 :       saturating = true;
+    2338             :     }
+    2339       84480 :     if (mpc_u(2) > max_snap_z * 1.01) {
+    2340           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap Z: " << mpc_u(2));
+    2341           0 :       mpc_u(2)   = max_snap_z;
+    2342           0 :       saturating = true;
+    2343             :     }
+    2344       84480 :     if (mpc_u(2) < -min_snap_z * 1.01) {
+    2345           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap Z: " << mpc_u(2));
+    2346           0 :       mpc_u(2)   = -min_snap_z;
+    2347           0 :       saturating = true;
+    2348             :     }
+    2349             : 
+    2350       84480 :     if (saturating) {
+    2351           0 :       debugPrintState(0.1);
+    2352           0 :       debugPrintMPCResult(0.1);
+    2353             :     }
+    2354             :   }
+    2355             : 
+    2356             :   {
+    2357       84480 :     std::scoped_lock lock(mutex_mpc_u_);
+    2358             : 
+    2359       84480 :     mpc_u_         = mpc_u;
+    2360       84480 :     mpc_u_heading_ = mpc_u_heading;
+    2361             :   }
+    2362             : 
+    2363       84480 :   double mpc_solver_time = (ros::Time::now() - time_begin).toSec();
+    2364       84480 :   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_) {
+    2365           1 :     ROS_DEBUG_STREAM_THROTTLE(1.0, "[MpcTracker]: Total MPC solver time: " << mpc_solver_time << " iters X: " << iters_x << "/" << _max_iters_xy_
+    2366             :                                                                            << " iters Y:  " << iters_y << "/" << _max_iters_xy_ << " iters Z: " << iters_z
+    2367             :                                                                            << "/" << _max_iters_z_ << " iters heading: " << iters_heading << "/"
+    2368             :                                                                            << _max_iters_heading_);
+    2369             :   }
+    2370             : 
+    2371       84480 :   future_was_predicted_ = true;
+    2372             : 
+    2373             :   // | ------------- breaking for the next iteration ------------ |
+    2374             : 
+    2375      159716 :   if (drs_params.braking_enabled && (std::abs(des_x_filtered(MPC_HORIZON_LENGTH - 6) - des_x_filtered(MPC_HORIZON_LENGTH - 1)) <= 0.1) &&
+    2376      145860 :       (std::abs(des_y_filtered(MPC_HORIZON_LENGTH - 6) - des_y_filtered(MPC_HORIZON_LENGTH - 1)) <= 0.1) &&
+    2377      239584 :       (std::abs(des_z_filtered(MPC_HORIZON_LENGTH - 6) - des_z_filtered(MPC_HORIZON_LENGTH - 1)) <= 0.1) &&
+    2378       68978 :       (std::abs(radians::diff(des_heading_trajectory(MPC_HORIZON_LENGTH - 6), des_heading_trajectory(MPC_HORIZON_LENGTH - 1))) <= 0.1)) {
+    2379             : 
+    2380       68762 :     brake_ = true;
+    2381       68762 :     ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: braking");
+    2382             : 
+    2383             :   } else {
+    2384       15718 :     brake_ = false;
+    2385             :   }
+    2386             : 
+    2387             :   /* publish mpc reference //{ */
+    2388             : 
+    2389             :   {
+    2390      168960 :     geometry_msgs::PoseArray debug_trajectory_out;
+    2391       84480 :     debug_trajectory_out.header.stamp    = ros::Time::now();
+    2392       84480 :     debug_trajectory_out.header.frame_id = uav_state_.header.frame_id;
+    2393             : 
+    2394             :     {
+    2395      168960 :       std::scoped_lock lock(mutex_predicted_trajectory_);
+    2396             : 
+    2397     3463676 :       for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    2398             : 
+    2399     3379200 :         geometry_msgs::Pose new_pose;
+    2400             : 
+    2401     3379200 :         new_pose.position.x = des_x_filtered(i, 0);
+    2402     3379200 :         new_pose.position.y = des_y_filtered(i, 0);
+    2403     3379200 :         new_pose.position.z = des_z_filtered(i, 0);
+    2404             : 
+    2405     3379200 :         new_pose.orientation = mrs_lib::AttitudeConverter(0, 0, des_heading_trajectory(i));
+    2406             : 
+    2407     3379200 :         debug_trajectory_out.poses.push_back(new_pose);
+    2408             :       }
+    2409             :     }
+    2410             : 
+    2411       84480 :     ph_mpc_reference_debugging_.publish(debug_trajectory_out);
+    2412             :   }
+    2413             : 
+    2414             :   //}
+    2415             : }
+    2416             : 
+    2417             : //}
+    2418             : 
+    2419             : /* iterateModel() //{ */
+    2420             : 
+    2421       81105 : void MpcTracker::iterateModel(const double& dt) {
+    2422             : 
+    2423       81105 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    2424             : 
+    2425       81105 :   if (model_first_iteration_) {
+    2426             : 
+    2427         100 :     model_iteration_last_time_ = ros::Time::now();
+    2428         100 :     model_first_iteration_     = false;
+    2429             : 
+    2430             :   } else {
+    2431             : 
+    2432       81005 :     dt1 = 0.9 * dt1 + 0.1 * dt;
+    2433             : 
+    2434       81005 :     mrs_lib::set_mutexed(mutex_dt1_, dt1, dt1_);
+    2435       81005 :     timer_mpc_iteration_.setPeriod(ros::Duration(dt1), false);
+    2436             : 
+    2437             :     // clang-format off
+    2438       81005 :     A_ << 1, dt1, 0.5*dt1*dt1, 0,           0, 0,   0,           0,           0, 0,   0,           0,
+    2439       81005 :           0, 1,   dt1,         0.5*dt1*dt1, 0, 0,   0,           0,           0, 0,   0,           0,
+    2440       81005 :           0, 0,   1,           dt1,         0, 0,   0,           0,           0, 0,   0,           0,
+    2441       81005 :           0, 0,   0,           1,           0, 0,   0,           0,           0, 0,   0,           0,
+    2442       81005 :           0, 0,   0,           0,           1, dt1, 0.5*dt1*dt1, 0,           0, 0,   0,           0,
+    2443       81005 :           0, 0,   0,           0,           0, 1,   dt1,         0.5*dt1*dt1, 0, 0,   0,           0,
+    2444       81005 :           0, 0,   0,           0,           0, 0,   1,           dt1,         0, 0,   0,           0,
+    2445       81005 :           0, 0,   0,           0,           0, 0,   0,           1,           0, 0,   0,           0,
+    2446       81005 :           0, 0,   0,           0,           0, 0,   0,           0,           1, dt1, 0.5*dt1*dt1, 0,
+    2447       81005 :           0, 0,   0,           0,           0, 0,   0,           0,           0, 1,   dt1,         0.5*dt1*dt1,
+    2448       81005 :           0, 0,   0,           0,           0, 0,   0,           0,           0, 0,   1,           dt1,
+    2449       81005 :           0, 0,   0,           0,           0, 0,   0,           0,           0, 0,   0,           1;
+    2450             : 
+    2451       81005 :       B_ << 0,   0,   0,
+    2452       81005 :             0,   0,   0,
+    2453       81005 :             0,   0,   0,
+    2454       81005 :             dt1, 0,   0,
+    2455       81005 :             0,   0,   0,
+    2456       81005 :             0,   0,   0,
+    2457       81005 :             0,   0,   0,
+    2458       81005 :             0,   dt1, 0,
+    2459       81005 :             0,   0,   0,
+    2460       81005 :             0,   0,   0,
+    2461       81005 :             0,   0,   0,
+    2462       81005 :             0,   0,   dt1;
+    2463             : 
+    2464       81005 :       A_heading_ << 1, dt1, 0.5*dt1*dt1, 0,
+    2465       81005 :                     0, 1,   dt1,         0.5*dt1*dt1,
+    2466       81005 :                     0, 0,   1,           dt1,
+    2467       81005 :                     0, 0,   0,           1;
+    2468             : 
+    2469       81005 :       B_heading_ << 0,
+    2470       81005 :                     0,
+    2471      162010 :                     0,
+    2472       81005 :                     dt1;
+    2473             : 
+    2474       81005 :     model_iteration_last_time_ = ros::Time::now();
+    2475             :   }
+    2476             : 
+    2477             :   {
+    2478      162210 :     auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    2479      162210 :     auto [mpc_u, mpc_u_heading] = mrs_lib::get_mutexed(mutex_mpc_u_, mpc_u_, mpc_u_heading_);
+    2480             : 
+    2481      162210 :     MatrixXd new_mpc_x         = A_ * mpc_x + B_ * mpc_u;
+    2482      162210 :     MatrixXd new_mpc_x_heading = A_heading_ * mpc_x_heading + B_heading_ * mpc_u_heading;
+    2483             : 
+    2484             :     // | --------------- check the state difference --------------- |
+    2485             :     {
+    2486       81105 :       auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    2487             : 
+    2488       81105 :       bool problem = false;
+    2489             : 
+    2490             :       // position
+    2491             : 
+    2492       81105 :       if (fabs((new_mpc_x(0) - mpc_x(0)) / dt1) > 1.05 * constraints.horizontal_speed) {
+    2493           0 :         ROS_DEBUG("[MpcTracker]: horizontal pos x update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(0), new_mpc_x(0),
+    2494             :                   fabs((new_mpc_x(0) - mpc_x(0)) / dt1), constraints.horizontal_speed);
+    2495           0 :         problem = true;
+    2496             :       }
+    2497             : 
+    2498       81105 :       if (fabs((new_mpc_x(4) - mpc_x(4)) / dt1) > 1.05 * constraints.horizontal_speed) {
+    2499           0 :         ROS_DEBUG("[MpcTracker]: horizontal pos y update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(4), new_mpc_x(4),
+    2500             :                   fabs((new_mpc_x(4) - mpc_x(4)) / dt1), constraints.horizontal_speed);
+    2501           0 :         problem = true;
+    2502             :     }
+    2503             : 
+    2504       81105 :       if (((new_mpc_x(8) - mpc_x(8)) / dt1) > 1.05 * constraints.vertical_ascending_speed) {
+    2505           0 :         ROS_DEBUG("[MpcTracker]: vertical pos z update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(8), new_mpc_x(8),
+    2506             :                   ((new_mpc_x(8) - mpc_x(8)) / dt1), constraints.vertical_ascending_speed);
+    2507           0 :         problem = true;
+    2508             :       }
+    2509             : 
+    2510       81105 :       if (((new_mpc_x(8) - mpc_x(8)) / dt1) < 1.05 * -constraints.vertical_descending_speed) {
+    2511           0 :         ROS_DEBUG("[MpcTracker]: vertical pos z update violates constraints: %.2f -> %.2f = %.2f, < %.2f", mpc_x(8), new_mpc_x(8),
+    2512             :                   ((new_mpc_x(8) - mpc_x(8)) / dt1), -constraints.vertical_descending_speed);
+    2513           0 :         problem = true;
+    2514             :       }
+    2515             : 
+    2516             :       /* if (fabs(radians::diff(new_mpc_x_heading(0), mpc_x_heading(0)) / dt1) > 1.2 * constraints.heading_speed) { */
+    2517             :       /*   ROS_DEBUG("[MpcTracker]: heading update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x_heading(0), new_mpc_x_heading(0), */
+    2518             :       /*             fabs(radians::diff(new_mpc_x_heading(0), mpc_x_heading(0)) / dt1), constraints.heading_speed); */
+    2519             :       /*   problem = true; */
+    2520             :       /* } */
+    2521             : 
+    2522             :       // velocity
+    2523             : 
+    2524       81105 :       if (fabs((new_mpc_x(1) - mpc_x(1)) / dt1) > 1.05 * constraints.horizontal_acceleration) {
+    2525           0 :         ROS_DEBUG("[MpcTracker]: horizontal vel x update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(1), new_mpc_x(1),
+    2526             :                   fabs((new_mpc_x(1) - mpc_x(1)) / dt1), constraints.horizontal_acceleration);
+    2527           0 :         problem = true;
+    2528             :       }
+    2529             : 
+    2530       81105 :       if (fabs((new_mpc_x(5) - mpc_x(5)) / dt1) > 1.05 * constraints.horizontal_acceleration) {
+    2531           0 :         ROS_DEBUG("[MpcTracker]: horizontal vel y update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(5), new_mpc_x(5),
+    2532             :                   fabs((new_mpc_x(5) - mpc_x(5)) / dt1), constraints.horizontal_acceleration);
+    2533           0 :         problem = true;
+    2534             :       }
+    2535             : 
+    2536       81105 :       if (((new_mpc_x(9) - mpc_x(9)) / dt1) > 1.05 * constraints.vertical_ascending_acceleration) {
+    2537           0 :         ROS_DEBUG("[MpcTracker]: vertical vel z update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(9), new_mpc_x(9),
+    2538             :                   ((new_mpc_x(9) - mpc_x(9)) / dt1), constraints.vertical_ascending_acceleration);
+    2539           0 :         problem = true;
+    2540             :       }
+    2541             : 
+    2542       81105 :       if (((new_mpc_x(9) - mpc_x(9)) / dt1) < 1.05 * -constraints.vertical_descending_acceleration) {
+    2543           0 :         ROS_DEBUG("[MpcTracker]: vertical vel z update violates constraints: %.2f -> %.2f = %.2f, < %.2f", mpc_x(9), new_mpc_x(9),
+    2544             :                   ((new_mpc_x(9) - mpc_x(9)) / dt1), -constraints.vertical_descending_acceleration);
+    2545           0 :         problem = true;
+    2546             :       }
+    2547             : 
+    2548             :       // acceleration
+    2549             : 
+    2550       81105 :       if (fabs((new_mpc_x(2) - mpc_x(2)) / dt1) > 1.05 * constraints.horizontal_jerk) {
+    2551           0 :         ROS_DEBUG("[MpcTracker]: horizontal acc x update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(2), new_mpc_x(2),
+    2552             :                   fabs((new_mpc_x(2) - mpc_x(2)) / dt1), constraints.horizontal_jerk);
+    2553           0 :         problem = true;
+    2554             :       }
+    2555             : 
+    2556       81105 :       if (fabs((new_mpc_x(6) - mpc_x(6)) / dt1) > 1.05 * constraints.horizontal_jerk) {
+    2557           0 :         ROS_DEBUG("[MpcTracker]: horizontal acc y update violates constraints: %.2f -> %.2f, = %.2f > %.2f", mpc_x(6), new_mpc_x(6),
+    2558             :                   fabs((new_mpc_x(6) - mpc_x(6)) / dt1), constraints.horizontal_jerk);
+    2559           0 :         problem = true;
+    2560             :       }
+    2561             : 
+    2562       81105 :       if (((new_mpc_x(10) - mpc_x(10)) / dt1) > 1.05 * constraints.vertical_ascending_jerk) {
+    2563           0 :         ROS_DEBUG("[MpcTracker]: vertical acc z update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(10), new_mpc_x(10),
+    2564             :                   ((new_mpc_x(10) - mpc_x(10)) / dt1), constraints.vertical_ascending_jerk);
+    2565           0 :         problem = true;
+    2566             :       }
+    2567             : 
+    2568       81105 :       if (((new_mpc_x(10) - mpc_x(10)) / dt1) < 1.05 * -constraints.vertical_descending_jerk) {
+    2569           0 :         ROS_DEBUG("[MpcTracker]: vertical acc z update violates constraints: %.2f -> %.2f = %.2f, < %.2f", mpc_x(10), new_mpc_x(10),
+    2570             :                   ((new_mpc_x(10) - mpc_x(10)) / dt1), -constraints.vertical_descending_jerk);
+    2571           0 :         problem = true;
+    2572             :       }
+    2573             : 
+    2574             :       // jerk
+    2575             : 
+    2576       81105 :       if (fabs((new_mpc_x(3) - mpc_x(3)) / dt1) > 1.05 * constraints.horizontal_snap) {
+    2577           0 :         ROS_DEBUG("[MpcTracker]: horizontal jerk x update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(3), new_mpc_x(3),
+    2578             :                   fabs((new_mpc_x(3) - mpc_x(3)) / dt1), constraints.horizontal_snap);
+    2579           0 :         problem = true;
+    2580             :       }
+    2581             : 
+    2582       81105 :       if (fabs((new_mpc_x(7) - mpc_x(7)) / dt1) > 1.05 * constraints.horizontal_snap) {
+    2583           0 :         ROS_DEBUG("[MpcTracker]: horizontal jerk y update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(7), new_mpc_x(7),
+    2584             :                   fabs((new_mpc_x(7) - mpc_x(7)) / dt1), constraints.horizontal_snap);
+    2585           0 :         problem = true;
+    2586             :       }
+    2587             : 
+    2588       81105 :       if (((new_mpc_x(11) - mpc_x(11)) / dt1) > 1.05 * constraints.vertical_ascending_snap) {
+    2589           0 :         ROS_DEBUG("[MpcTracker]: vertical jerk z update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(11), new_mpc_x(11),
+    2590             :                   ((new_mpc_x(11) - mpc_x(11)) / dt1), constraints.vertical_ascending_snap);
+    2591           0 :         problem = true;
+    2592             :       }
+    2593             : 
+    2594       81105 :       if (((new_mpc_x(11) - mpc_x(11)) / dt1) < 1.05 * -constraints.vertical_descending_snap) {
+    2595           0 :         ROS_DEBUG("[MpcTracker]: vertical jerk z update violates constraints: %.2f -> %.2f = %.2f, < %.2f", mpc_x(11), new_mpc_x(11),
+    2596             :                   ((new_mpc_x(11) - mpc_x(11)) / dt1), -constraints.vertical_descending_snap);
+    2597           0 :         problem = true;
+    2598             :       }
+    2599             : 
+    2600       81105 :       if (problem) {
+    2601           0 :         debugPrintState(0.001);
+    2602           0 :         debugPrintMPCResult(0.001);
+    2603             :       }
+    2604             :     }
+    2605             : 
+    2606             :     {
+    2607       81105 :       std::scoped_lock lock(mutex_mpc_x_);
+    2608             : 
+    2609       81105 :       mpc_x_         = new_mpc_x;
+    2610       81105 :       mpc_x_heading_ = new_mpc_x_heading;
+    2611             : 
+    2612       81105 :       mpc_x_heading_(0) = sradians::wrap(mpc_x_heading_(0));
+    2613             :     }
+    2614             :   }
+    2615       81105 : }
+    2616             : 
+    2617             : //}
+    2618             : 
+    2619             : // | -------------------- referece setting -------------------- |
+    2620             : 
+    2621             : /* //{ loadTrajectory() */
+    2622             : 
+    2623             : // method for setting desired trajectory
+    2624         824 : std::tuple<bool, std::string, bool> MpcTracker::loadTrajectory(const mrs_msgs::TrajectoryReference msg) {
+    2625             : 
+    2626         824 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    2627             : 
+    2628             :   // copy the member variables
+    2629        1648 :   auto x         = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    2630        1648 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    2631             : 
+    2632        1648 :   std::stringstream ss;
+    2633             : 
+    2634             :   /* check the trajectory dt //{ */
+    2635             : 
+    2636             :   double trajectory_dt;
+    2637         824 :   if (msg.dt <= 1e-4) {
+    2638           0 :     trajectory_dt = 0.2;
+    2639           0 :     ROS_WARN_THROTTLE(10.0, "[MpcTracker]: the trajectory dt was not specified, assuming its the old 0.2 s");
+    2640         824 :   } else if (msg.dt < dt1) {
+    2641           0 :     trajectory_dt = 0.2;
+    2642           0 :     ss << std::setprecision(3) << "the trajectory dt (" << msg.dt << " s) is too small (smaller than the tracker's internal step size: " << dt1 << " s)";
+    2643           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    2644           0 :     return std::tuple(false, ss.str(), false);
+    2645             :   } else {
+    2646         824 :     trajectory_dt = msg.dt;
+    2647             :   }
+    2648             : 
+    2649             :   //}
+    2650             : 
+    2651         824 :   int trajectory_size = msg.points.size();
+    2652             : 
+    2653             :   /* sanitize the time-ness of the trajectory //{ */
+    2654             : 
+    2655         824 :   int    trajectory_sample_offset    = 0;  // how many samples in past is the trajectory
+    2656         824 :   int    trajectory_subsample_offset = 0;  // how many simulation inner loops ahead of the first valid sample
+    2657         824 :   double trajectory_time_offset      = 0;  // how much time in past in [s]
+    2658             : 
+    2659             :   // btw, "trajectory_time_offset = trajectory_dt*trajectory_sample_offset + _dt1_*trajectory_subsample_offset" should hold
+    2660         824 :   if (msg.fly_now) {
+    2661             : 
+    2662         823 :     ros::Time trajectory_time = msg.header.stamp;
+    2663             : 
+    2664             :     // the desired time is 0 => the current time
+    2665             :     // the trajecoty is a single point => the current time
+    2666         823 :     if (trajectory_time == ros::Time(0) || int(msg.points.size()) == 1) {
+    2667             : 
+    2668           3 :       trajectory_time_offset = 0.0;
+    2669             : 
+    2670             :       // the desired time is specified
+    2671             :     } else {
+    2672             : 
+    2673         820 :       trajectory_time_offset = (ros::Time::now() - trajectory_time).toSec();
+    2674             : 
+    2675             :       // when the time offset is negative, thus in the future
+    2676             :       // just say it, but use it like its from the current time
+    2677         820 :       if (trajectory_time_offset < 0.0) {
+    2678             : 
+    2679           0 :         ROS_WARN_THROTTLE(1.0, "[MpcTracker]: received trajectory with timestamp in the future by %.3f s", -trajectory_time_offset);
+    2680             : 
+    2681           0 :         trajectory_time_offset = 0.0;
+    2682             :       }
+    2683             :     }
+    2684             : 
+    2685             :     // if the time offset is set, check if we need to "move the first idx"
+    2686         823 :     if (trajectory_time_offset > 0.0) {
+    2687             : 
+    2688             :       // calculate the offset in samples
+    2689           2 :       trajectory_sample_offset = int(floor(trajectory_time_offset / trajectory_dt));
+    2690             : 
+    2691             :       // and get the subsample offset, which will be used to initialize the interpolator
+    2692           2 :       trajectory_subsample_offset = int(floor(fmod(trajectory_time_offset, trajectory_dt) / dt1));
+    2693             : 
+    2694           2 :       ROS_DEBUG_THROTTLE(0.1, "[MpcTracker]: received trajectory with timestamp in the past by %.3f s",
+    2695             :                          trajectory_dt * trajectory_sample_offset + dt1 * trajectory_subsample_offset);
+    2696             : 
+    2697             :       // if the offset is larger than the number of points in the trajectory
+    2698             :       // the trajectory can not be used
+    2699           2 :       if (trajectory_sample_offset >= trajectory_size) {
+    2700             : 
+    2701           0 :         ss << "trajectory timestamp is too old (time difference = " << trajectory_time_offset << ")";
+    2702           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    2703           0 :         return std::tuple(false, ss.str(), false);
+    2704             : 
+    2705             :       } else {
+    2706             : 
+    2707             :         // if the offset is larger than one trajectory sample,
+    2708             :         // offset the start
+    2709           2 :         if (trajectory_time_offset >= trajectory_dt) {
+    2710             : 
+    2711             :           // decrease the trajectory size
+    2712           0 :           trajectory_size -= trajectory_sample_offset;
+    2713             : 
+    2714           0 :           ROS_DEBUG_THROTTLE(0.1, "[MpcTracker]: offsetting trajectory by %d samples", trajectory_sample_offset);
+    2715             : 
+    2716             :         } else {
+    2717             : 
+    2718           2 :           trajectory_sample_offset = 0;
+    2719             :         }
+    2720             :       }
+    2721             :     }
+    2722             : 
+    2723             :   } else { // not fly_now
+    2724             : 
+    2725           1 :       trajectory_tracking_in_progress_ = false;
+    2726             :   }
+    2727             : 
+    2728             :   //}
+    2729             : 
+    2730         824 :   ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: trajectory sample offset: %d", trajectory_sample_offset);
+    2731         824 :   ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: trajectory subsample offset: %d", trajectory_subsample_offset);
+    2732             : 
+    2733             :   // after this, we should have the correct value of
+    2734             :   // * trajectory_size
+    2735             :   // * trajectory_sample_offset
+    2736             :   // * trajectory_subsample_offset
+    2737             : 
+    2738             :   /* copy the trajectory to a local variable //{ */
+    2739             : 
+    2740             :   // copy only the part from the first valid index
+    2741             : 
+    2742        1648 :   MatrixXd des_x_whole_trajectory       = VectorXd::Zero(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2743        1648 :   MatrixXd des_y_whole_trajectory       = VectorXd::Zero(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2744        1648 :   MatrixXd des_z_whole_trajectory       = VectorXd::Zero(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2745        1648 :   MatrixXd des_heading_whole_trajectory = VectorXd::Zero(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2746             : 
+    2747       42418 :   for (int i = 0; i < trajectory_size; i++) {
+    2748             : 
+    2749       41594 :     des_x_whole_trajectory(i)       = msg.points.at(trajectory_sample_offset + i).position.x;
+    2750       41594 :     des_y_whole_trajectory(i)       = msg.points.at(trajectory_sample_offset + i).position.y;
+    2751       41594 :     des_z_whole_trajectory(i)       = msg.points.at(trajectory_sample_offset + i).position.z;
+    2752       41594 :     des_heading_whole_trajectory(i) = msg.points.at(trajectory_sample_offset + i).heading;
+    2753             :   }
+    2754             : 
+    2755             :   //}
+    2756             : 
+    2757             :   /* set looping //{ */
+    2758             : 
+    2759         824 :   bool loop = false;
+    2760             : 
+    2761         824 :   if (msg.loop) {
+    2762             : 
+    2763           0 :     double first_x = des_x_whole_trajectory(0);
+    2764           0 :     double first_y = des_y_whole_trajectory(0);
+    2765           0 :     double first_z = des_z_whole_trajectory(0);
+    2766           0 :     double first_hdg = des_heading_whole_trajectory(0);
+    2767             : 
+    2768           0 :     double last_x = des_x_whole_trajectory(trajectory_size - 1);
+    2769           0 :     double last_y = des_y_whole_trajectory(trajectory_size - 1);
+    2770           0 :     double last_z = des_z_whole_trajectory(trajectory_size - 1);
+    2771           0 :     double last_hdg = des_heading_whole_trajectory(trajectory_size - 1);
+    2772             : 
+    2773             :     // check whether the trajectory is loopable
+    2774           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) {
+    2775             : 
+    2776           0 :       ROS_INFO_THROTTLE(1.0, "[MpcTracker]: looping enabled");
+    2777           0 :       loop = true;
+    2778             : 
+    2779             :     } else {
+    2780             : 
+    2781           0 :       ss << "can not loop trajectory, the first and last points are too far apart";
+    2782           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    2783           0 :       return std::tuple(false, ss.str(), false);
+    2784             :     }
+    2785             : 
+    2786             :   } else {
+    2787             : 
+    2788         824 :     loop = false;
+    2789             :   }
+    2790             : 
+    2791             :   //}
+    2792             : 
+    2793             :   // by this time, the values of these should be set:
+    2794             :   // * loop
+    2795             : 
+    2796             :   /* add tail (the last point repeated to fill the prediction horizon) //{ */
+    2797             : 
+    2798         824 :   if (!loop) {
+    2799             : 
+    2800             :     // extend it so it has smooth ending
+    2801       33784 :     for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    2802             : 
+    2803       32960 :       des_x_whole_trajectory(i + trajectory_size)       = des_x_whole_trajectory(i + trajectory_size - 1);
+    2804       32960 :       des_y_whole_trajectory(i + trajectory_size)       = des_y_whole_trajectory(i + trajectory_size - 1);
+    2805       32960 :       des_z_whole_trajectory(i + trajectory_size)       = des_z_whole_trajectory(i + trajectory_size - 1);
+    2806       32960 :       des_heading_whole_trajectory(i + trajectory_size) = des_heading_whole_trajectory(i + trajectory_size - 1);
+    2807             :     }
+    2808             :   }
+    2809             : 
+    2810             :   //}
+    2811             : 
+    2812             :   // by this time, the values of these should be set correctly:
+    2813             :   // * trajectory_size
+    2814             :   // * des_x_whole_trajectory
+    2815             :   // * des_y_whole_trajectory
+    2816             :   // * des_z_whole_trajectory
+    2817             :   // * des_heading_whole_trajectory
+    2818             : 
+    2819             :   /* update the global variables //{ */
+    2820             : 
+    2821             :   {
+    2822        1648 :     std::scoped_lock lock(mutex_des_whole_trajectory_, mutex_des_trajectory_, mutex_trajectory_tracking_states_);
+    2823             : 
+    2824         824 :     des_whole_trajectory_id_ = msg.input_id;
+    2825             : 
+    2826         824 :     auto mpc_x_heading = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_heading_);
+    2827             : 
+    2828         824 :     trajectory_tracking_in_progress_ = msg.fly_now;
+    2829         824 :     trajectory_track_heading_        = msg.use_heading;
+    2830             : 
+    2831             :     // allocate the vectors
+    2832         824 :     des_x_whole_trajectory_       = std::make_shared<VectorXd>(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2833         824 :     des_y_whole_trajectory_       = std::make_shared<VectorXd>(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2834         824 :     des_z_whole_trajectory_       = std::make_shared<VectorXd>(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2835         824 :     des_heading_whole_trajectory_ = std::make_shared<VectorXd>(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2836             : 
+    2837       75378 :     for (int i = 0; i < trajectory_size + MPC_HORIZON_LENGTH; i++) {
+    2838             : 
+    2839       74554 :       (*des_x_whole_trajectory_)(i) = des_x_whole_trajectory(i);
+    2840       74554 :       (*des_y_whole_trajectory_)(i) = des_y_whole_trajectory(i);
+    2841       74554 :       (*des_z_whole_trajectory_)(i) = des_z_whole_trajectory(i);
+    2842             : 
+    2843       74554 :       if (trajectory_track_heading_) {
+    2844       74554 :         (*des_heading_whole_trajectory_)(i) = des_heading_whole_trajectory(i);
+    2845             :       } else {
+    2846           0 :         (*des_heading_whole_trajectory_).fill(mpc_x_heading(0, 0));
+    2847             :       }
+    2848             :     }
+    2849             : 
+    2850             :     // if we are tracking trajectory, copy the setpoint
+    2851         824 :     if (trajectory_tracking_in_progress_) {
+    2852             : 
+    2853         823 :       toggleHover(false);
+    2854             : 
+    2855             :       /* interpolate the trajectory points and fill in the desired_trajectory vector //{ */
+    2856             : 
+    2857       33743 :       for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    2858             : 
+    2859       32920 :         double first_time = dt1 + i * _dt2_ + trajectory_subsample_offset * dt1;
+    2860             : 
+    2861       32920 :         int first_idx  = floor(first_time / trajectory_dt);
+    2862       32920 :         int second_idx = first_idx + 1;
+    2863             : 
+    2864       32920 :         double interp_coeff = std::fmod(first_time / trajectory_dt, 1.0);
+    2865             : 
+    2866       32920 :         if (trajectory_tracking_loop_) {
+    2867             : 
+    2868           0 :           if (second_idx >= trajectory_size) {
+    2869           0 :             second_idx -= trajectory_size;
+    2870             :           }
+    2871             : 
+    2872           0 :           if (first_idx >= trajectory_size) {
+    2873           0 :             first_idx -= trajectory_size;
+    2874             :           }
+    2875             :         } else {
+    2876             : 
+    2877       32920 :           if (second_idx >= trajectory_size) {
+    2878           5 :             second_idx = trajectory_size - 1;
+    2879             :           }
+    2880             : 
+    2881       32920 :           if (first_idx >= trajectory_size) {
+    2882           4 :             first_idx = trajectory_size - 1;
+    2883             :           }
+    2884             :         }
+    2885             : 
+    2886       32920 :         des_x_trajectory_(i, 0) = (1.0 - interp_coeff) * des_x_whole_trajectory(first_idx) + interp_coeff * des_x_whole_trajectory(second_idx);
+    2887       32920 :         des_y_trajectory_(i, 0) = (1.0 - interp_coeff) * des_y_whole_trajectory(first_idx) + interp_coeff * des_y_whole_trajectory(second_idx);
+    2888       32920 :         des_z_trajectory_(i, 0) = (1.0 - interp_coeff) * des_z_whole_trajectory(first_idx) + interp_coeff * des_z_whole_trajectory(second_idx);
+    2889             : 
+    2890       32920 :         des_heading_trajectory_(i, 0) = sradians::interp(des_heading_whole_trajectory(first_idx), des_heading_whole_trajectory(second_idx), interp_coeff);
+    2891             :       }
+    2892             : 
+    2893             :       //}
+    2894             :     }
+    2895             : 
+    2896         824 :     trajectory_size_             = trajectory_size;
+    2897         824 :     trajectory_current_time_     = 0;
+    2898         824 :     trajectory_set_              = true;
+    2899         824 :     trajectory_tracking_loop_    = loop;
+    2900         824 :     trajectory_dt_               = trajectory_dt;
+    2901         824 :     trajectory_count_++;
+    2902             :   }
+    2903             : 
+    2904             :   //}
+    2905             : 
+    2906         824 :   ROS_INFO_THROTTLE(1, "[MpcTracker]: finished setting trajectory with length %d", trajectory_size);
+    2907             : 
+    2908             :   /* publish the debugging topics of the post-processed trajectory //{ */
+    2909             : 
+    2910             :   {
+    2911             : 
+    2912        1648 :     geometry_msgs::PoseArray debug_trajectory_out;
+    2913         824 :     debug_trajectory_out.header.stamp    = ros::Time::now();
+    2914         824 :     debug_trajectory_out.header.frame_id = common_handlers_->transformer->resolveFrame(msg.header.frame_id);
+    2915             : 
+    2916             :     {
+    2917        1648 :       std::scoped_lock lock(mutex_des_whole_trajectory_);
+    2918             : 
+    2919       42418 :       for (int i = 0; i < trajectory_size; i++) {
+    2920             : 
+    2921       41594 :         geometry_msgs::Pose new_pose;
+    2922             : 
+    2923       41594 :         new_pose.position.x = (*des_x_whole_trajectory_)(i);
+    2924       41594 :         new_pose.position.y = (*des_y_whole_trajectory_)(i);
+    2925       41594 :         new_pose.position.z = (*des_z_whole_trajectory_)(i);
+    2926             : 
+    2927       41594 :         new_pose.orientation = mrs_lib::AttitudeConverter(0, 0, (*des_heading_whole_trajectory_)(i));
+    2928             : 
+    2929       41594 :         debug_trajectory_out.poses.push_back(new_pose);
+    2930             :       }
+    2931             :     }
+    2932             : 
+    2933         824 :     pub_debug_processed_trajectory_poses_.publish(debug_trajectory_out);
+    2934             : 
+    2935        1648 :     visualization_msgs::MarkerArray msg_out;
+    2936             : 
+    2937        1648 :     visualization_msgs::Marker marker;
+    2938             : 
+    2939         824 :     marker.header.stamp     = ros::Time::now();
+    2940         824 :     marker.header.frame_id  = common_handlers_->transformer->resolveFrame(msg.header.frame_id);
+    2941         824 :     marker.type             = visualization_msgs::Marker::LINE_LIST;
+    2942         824 :     marker.color.a          = 1;
+    2943         824 :     marker.scale.x          = 0.05;
+    2944         824 :     marker.color.r          = 1;
+    2945         824 :     marker.color.g          = 0;
+    2946         824 :     marker.color.b          = 0;
+    2947         824 :     marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2948             : 
+    2949             :     {
+    2950        1648 :       std::scoped_lock lock(mutex_des_whole_trajectory_);
+    2951             : 
+    2952       41594 :       for (int i = 0; i < trajectory_size - 1; i++) {
+    2953             : 
+    2954       40770 :         geometry_msgs::Point point1;
+    2955             : 
+    2956       40770 :         point1.x = des_x_whole_trajectory(i);
+    2957       40770 :         point1.y = des_y_whole_trajectory(i);
+    2958       40770 :         point1.z = des_z_whole_trajectory(i);
+    2959             : 
+    2960       40770 :         marker.points.push_back(point1);
+    2961             : 
+    2962       40770 :         geometry_msgs::Point point2;
+    2963             : 
+    2964       40770 :         point2.x = des_x_whole_trajectory(i + 1);
+    2965       40770 :         point2.y = des_y_whole_trajectory(i + 1);
+    2966       40770 :         point2.z = des_z_whole_trajectory(i + 1);
+    2967             : 
+    2968       40770 :         marker.points.push_back(point2);
+    2969             :       }
+    2970             :     }
+    2971             : 
+    2972         824 :     msg_out.markers.push_back(marker);
+    2973             : 
+    2974         824 :     pub_debug_processed_trajectory_markers_.publish(msg_out);
+    2975             :   }
+    2976             : 
+    2977             :   //}
+    2978             : 
+    2979         824 :   publishDiagnostics();
+    2980             : 
+    2981        1648 :   return std::tuple(true, "trajectory loaded", false);
+    2982             : }
+    2983             : 
+    2984             : //}
+    2985             : 
+    2986             : /* //{ setSinglePointReference() */
+    2987             : 
+    2988             : // fill the des_*_trajectory based on a single point
+    2989         840 : void MpcTracker::setSinglePointReference(const double x, const double y, const double z, const double heading) {
+    2990             : 
+    2991        1680 :   std::scoped_lock lock(mutex_des_trajectory_);
+    2992             : 
+    2993         840 :   des_x_trajectory_.fill(x);
+    2994         840 :   des_y_trajectory_.fill(y);
+    2995         840 :   des_z_trajectory_.fill(z);
+    2996         840 :   des_heading_trajectory_.fill(heading);
+    2997         840 : }
+    2998             : 
+    2999             : //}
+    3000             : 
+    3001             : /* //{ setGoal() */
+    3002             : 
+    3003             : // set absolute goal
+    3004         271 : void MpcTracker::setGoal(const double pos_x, const double pos_y, const double pos_z, const double heading, const bool use_heading) {
+    3005             : 
+    3006         271 :   double desired_heading = sradians::wrap(heading);
+    3007             : 
+    3008         542 :   auto mpc_x_heading = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_heading_);
+    3009             : 
+    3010         271 :   if (!use_heading) {
+    3011           0 :     desired_heading = mpc_x_heading(0, 0);
+    3012             :   }
+    3013             : 
+    3014         271 :   trajectory_tracking_in_progress_ = false;
+    3015             : 
+    3016         271 :   setSinglePointReference(pos_x, pos_y, pos_z, desired_heading);
+    3017             : 
+    3018         271 :   publishDiagnostics();
+    3019         271 : }
+    3020             : 
+    3021             : //}
+    3022             : 
+    3023             : /* //{ setRelativeGoal() */
+    3024             : 
+    3025         569 : void MpcTracker::setRelativeGoal(const double pos_x, const double pos_y, const double pos_z, const double heading, const bool use_heading) {
+    3026             : 
+    3027        1138 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    3028             : 
+    3029         569 :   double abs_x = mpc_x(0, 0) + pos_x;
+    3030         569 :   double abs_y = mpc_x(4, 0) + pos_y;
+    3031         569 :   double abs_z = mpc_x(8, 0) + pos_z;
+    3032             : 
+    3033         569 :   double abs_heading = mpc_x_heading(0, 0);
+    3034             : 
+    3035         569 :   if (use_heading) {
+    3036           0 :     abs_heading += heading;
+    3037             :   }
+    3038             : 
+    3039         569 :   trajectory_tracking_in_progress_ = false;
+    3040             : 
+    3041         569 :   setSinglePointReference(abs_x, abs_y, abs_z, abs_heading);
+    3042             : 
+    3043         569 :   publishDiagnostics();
+    3044         569 : }
+    3045             : 
+    3046             : //}
+    3047             : 
+    3048             : /* toggleHover() //{ */
+    3049             : 
+    3050        1319 : void MpcTracker::toggleHover(bool in) {
+    3051             : 
+    3052        1319 :   if (in == false && hovering_in_progress_) {
+    3053             : 
+    3054         105 :     ROS_DEBUG("[MpcTracker]: stoppping the hover timer");
+    3055             : 
+    3056         105 :     timer_hover_.stop();
+    3057             : 
+    3058         105 :     hovering_in_progress_ = false;
+    3059             : 
+    3060        1214 :   } else if (in == true && !hovering_in_progress_) {
+    3061             : 
+    3062         105 :     ROS_DEBUG("[MpcTracker]: starting the hover timer");
+    3063             : 
+    3064         105 :     hovering_in_progress_ = true;
+    3065             : 
+    3066         105 :     timer_hover_.start();
+    3067             :   }
+    3068        1319 : }
+    3069             : 
+    3070             : //}
+    3071             : 
+    3072             : // | ------------------- trajectory tracking ------------------ |
+    3073             : 
+    3074             : /* startTrajectoryTrackingImpl() //{ */
+    3075             : 
+    3076           2 : std::tuple<bool, std::string> MpcTracker::startTrajectoryTrackingImpl(void) {
+    3077             : 
+    3078           4 :   std::stringstream ss;
+    3079             : 
+    3080           2 :   if (trajectory_set_) {
+    3081             : 
+    3082           2 :     toggleHover(false);
+    3083             : 
+    3084             :     {
+    3085           2 :       std::scoped_lock lock(mutex_des_trajectory_);
+    3086             : 
+    3087           2 :       trajectory_tracking_in_progress_ = true;
+    3088           2 :       trajectory_current_time_ = 0;
+    3089             :     }
+    3090             : 
+    3091           2 :     publishDiagnostics();
+    3092             : 
+    3093           2 :     ss << "trajectory tracking started";
+    3094           2 :     ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3095             : 
+    3096           2 :     return std::tuple(true, ss.str());
+    3097             : 
+    3098             :   } else {
+    3099             : 
+    3100           0 :     ss << "can not start trajectory tracking, the trajectory is not set";
+    3101           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3102             : 
+    3103           0 :     return std::tuple(false, ss.str());
+    3104             :   }
+    3105             : }
+    3106             : 
+    3107             : //}
+    3108             : 
+    3109             : /* resumeTrajectoryTrackingImpl() //{ */
+    3110             : 
+    3111           1 : std::tuple<bool, std::string> MpcTracker::resumeTrajectoryTrackingImpl(void) {
+    3112             : 
+    3113           2 :   std::stringstream ss;
+    3114             : 
+    3115           1 :   if (trajectory_set_) {
+    3116             : 
+    3117           1 :     toggleHover(false);
+    3118             : 
+    3119           1 :     int trajectory_tracking_idx = getCurrentTrajectoryIdx();
+    3120             : 
+    3121           1 :     if (trajectory_tracking_idx < (trajectory_size_ - 1)) {
+    3122             : 
+    3123             :       {
+    3124           2 :         std::scoped_lock lock(mutex_des_trajectory_);
+    3125             : 
+    3126           1 :         trajectory_tracking_in_progress_ = true;
+    3127             :       }
+    3128             : 
+    3129           1 :       ss << "trajectory tracking resumed";
+    3130           1 :       ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3131             : 
+    3132           1 :       publishDiagnostics();
+    3133             : 
+    3134           1 :       return std::tuple(true, ss.str());
+    3135             : 
+    3136             :     } else {
+    3137             : 
+    3138           0 :       ss << "can not resume trajectory tracking, trajectory is already finished";
+    3139           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3140             : 
+    3141           0 :       return std::tuple(false, ss.str());
+    3142             :     }
+    3143             : 
+    3144             :   } else {
+    3145             : 
+    3146           0 :     ss << "can not resume trajectory tracking, ther trajectory is not set";
+    3147           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3148             : 
+    3149           0 :     return std::tuple(false, ss.str());
+    3150             :   }
+    3151             : }
+    3152             : 
+    3153             : //}
+    3154             : 
+    3155             : /* stopTrajectoryTrackingImpl() //{ */
+    3156             : 
+    3157           1 : std::tuple<bool, std::string> MpcTracker::stopTrajectoryTrackingImpl(void) {
+    3158             : 
+    3159           1 :   std::stringstream ss;
+    3160             : 
+    3161           1 :   if (trajectory_tracking_in_progress_) {
+    3162             : 
+    3163           1 :     trajectory_tracking_in_progress_ = false;
+    3164             : 
+    3165           1 :     toggleHover(true);
+    3166             : 
+    3167           1 :     ss << "stopping trajectory tracking";
+    3168           1 :     ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3169             : 
+    3170           1 :     publishDiagnostics();
+    3171             : 
+    3172             :   } else {
+    3173             : 
+    3174           0 :     ss << "can not stop trajectory tracking, already at stop";
+    3175           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3176             :   }
+    3177             : 
+    3178           2 :   return std::tuple(true, ss.str());
+    3179             : }
+    3180             : 
+    3181             : //}
+    3182             : 
+    3183             : /* gotoTrajectoryStartImpl() //{ */
+    3184             : 
+    3185           2 : std::tuple<bool, std::string> MpcTracker::gotoTrajectoryStartImpl(void) {
+    3186             : 
+    3187           4 :   std::stringstream ss;
+    3188             : 
+    3189           2 :   if (trajectory_set_) {
+    3190             : 
+    3191           2 :     toggleHover(false);
+    3192             : 
+    3193           2 :     trajectory_tracking_in_progress_ = false;
+    3194             : 
+    3195             :     {
+    3196           4 :       std::scoped_lock lock(mutex_des_whole_trajectory_);
+    3197             : 
+    3198           2 :       setGoal((*des_x_whole_trajectory_)(0), (*des_y_whole_trajectory_)(0), (*des_z_whole_trajectory_)(0), (*des_heading_whole_trajectory_)(0),
+    3199           2 :               trajectory_track_heading_);
+    3200             :     }
+    3201             : 
+    3202           2 :     publishDiagnostics();
+    3203             : 
+    3204           2 :     ss << "flying to the start of the trajectory";
+    3205           2 :     ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3206             : 
+    3207           2 :     return std::tuple(true, ss.str());
+    3208             : 
+    3209             :   } else {
+    3210             : 
+    3211           0 :     ss << "can not fly to the start of the trajectory, the trajectory is not set";
+    3212           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3213             : 
+    3214           0 :     return std::tuple(false, ss.str());
+    3215             :   }
+    3216             : }
+    3217             : 
+    3218             : //}
+    3219             : 
+    3220             : // | ------------------------- support ------------------------ |
+    3221             : 
+    3222             : /* //{ publishDiagnostics() */
+    3223             : 
+    3224       24619 : void MpcTracker::publishDiagnostics(void) {
+    3225             : 
+    3226       49238 :   auto des_x_trajectory       = mrs_lib::get_mutexed(mutex_des_trajectory_, des_x_trajectory_);
+    3227       49238 :   auto des_y_trajectory       = mrs_lib::get_mutexed(mutex_des_trajectory_, des_y_trajectory_);
+    3228       49238 :   auto des_z_trajectory       = mrs_lib::get_mutexed(mutex_des_trajectory_, des_z_trajectory_);
+    3229       49238 :   auto des_heading_trajectory = mrs_lib::get_mutexed(mutex_des_trajectory_, des_heading_trajectory_);
+    3230             : 
+    3231       49238 :   mrs_msgs::MpcTrackerDiagnostics diagnostics;
+    3232             : 
+    3233       24619 :   diagnostics.header.stamp    = ros::Time::now();
+    3234       24619 :   diagnostics.header.frame_id = uav_state_.header.frame_id;
+    3235             : 
+    3236       24619 :   diagnostics.active = is_active_;
+    3237             : 
+    3238       24619 :   diagnostics.uav_name = _uav_name_;
+    3239             : 
+    3240       24619 :   diagnostics.collision_avoidance_active = collision_avoidance_enabled_;
+    3241       24619 :   diagnostics.avoiding_collision         = collision_avoidance_affecting_me_;
+    3242             : 
+    3243       24619 :   diagnostics.setpoint.position.x = des_x_trajectory(0, 0);
+    3244       24619 :   diagnostics.setpoint.position.y = des_y_trajectory(0, 0);
+    3245       24619 :   diagnostics.setpoint.position.z = des_z_trajectory(0, 0);
+    3246             : 
+    3247       24619 :   diagnostics.setpoint.orientation = mrs_lib::AttitudeConverter(0, 0, des_heading_trajectory(0, 0));
+    3248             : 
+    3249       49238 :   std::stringstream ss;
+    3250             : 
+    3251             :   {
+    3252       49238 :     std::scoped_lock lock(mutex_other_uav_diagnostics_);
+    3253             : 
+    3254             :     // fill in if other UAVs are sending their trajectories
+    3255       24619 :     std::map<std::string, mrs_msgs::MpcTrackerDiagnostics>::iterator u = other_uav_diagnostics_.begin();
+    3256             : 
+    3257       26828 :     while (u != other_uav_diagnostics_.end()) {
+    3258             : 
+    3259        2209 :       if (u->second.collision_avoidance_active) {
+    3260             : 
+    3261             :         // is the other's trajectory fresh enought?
+    3262        1482 :         if ((ros::Time::now() - u->second.header.stamp).toSec() < _collision_trajectory_timeout_) {
+    3263        1482 :           diagnostics.avoidance_active_uavs.push_back(u->first);
+    3264        1482 :           ss << u->first.c_str() << ", ";
+    3265             :         }
+    3266             :       }
+    3267             : 
+    3268        2209 :       u++;
+    3269             :     }
+    3270             :   }
+    3271             : 
+    3272       49238 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    3273             : 
+    3274       24619 :   if (ss.str().length() > 0) {
+    3275        1482 :     ROS_DEBUG_STREAM_THROTTLE(5.0, "[MpcTracker]: getting avoidance trajectories: " << ss.str());
+    3276       50223 :   } else if (collision_avoidance_enabled_ &&
+    3277       27086 :       (uav_state.estimator_horizontal == "lat_gps" || uav_state.estimator_horizontal == "lat_rtk")) {
+    3278       19036 :     ROS_DEBUG_THROTTLE(10.0, "[MpcTracker]: missing avoidance trajectories!");
+    3279             :   }
+    3280             : 
+    3281       24619 :   pub_diagnostics_.publish(diagnostics);
+    3282             : 
+    3283       49238 :   std_msgs::String string_msg;
+    3284             : 
+    3285       24619 :   if (diagnostics.avoidance_active_uavs.empty()) {
+    3286             : 
+    3287       23137 :     string_msg.data = "-id col_avoid I see: NOTHING";
+    3288             : 
+    3289             :   } else {
+    3290             : 
+    3291        1482 :     string_msg.data = "-id col_avoid I see: ";
+    3292             :   }
+    3293             : 
+    3294       24619 :   if (diagnostics.avoidance_active_uavs.size() <= 3) {
+    3295             : 
+    3296       26101 :     for (size_t i = 0; i < diagnostics.avoidance_active_uavs.size(); i++) {
+    3297        1482 :       if (i == 0) {
+    3298        1482 :         string_msg.data += diagnostics.avoidance_active_uavs.at(i);
+    3299             :       } else {
+    3300           0 :         string_msg.data += ", " + diagnostics.avoidance_active_uavs.at(i);
+    3301             :       }
+    3302             :     }
+    3303             : 
+    3304             :   } else {
+    3305             : 
+    3306           0 :     std::stringstream ss;
+    3307           0 :     ss << diagnostics.avoidance_active_uavs.size();
+    3308             : 
+    3309           0 :     string_msg.data += ss.str() + " UAVs";
+    3310             :   }
+    3311             : 
+    3312       24619 :   pub_status_string_.publish(string_msg);
+    3313       24619 : }
+    3314             : 
+    3315             : //}
+    3316             : 
+    3317             : /* debugPrintState() //{ */
+    3318             : 
+    3319           0 : void MpcTracker::debugPrintState(const double throttle) {
+    3320             : 
+    3321           0 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    3322             : 
+    3323           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));
+    3324           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));
+    3325           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));
+    3326           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));
+    3327           0 : }
+    3328             : 
+    3329             : //}
+    3330             : 
+    3331             : /* debugPrintMPCu() //{ */
+    3332             : 
+    3333           0 : void MpcTracker::debugPrintMPCResult(const double throttle) {
+    3334             : 
+    3335           0 :   auto [mpc_u, mpc_u_heading] = mrs_lib::get_mutexed(mutex_mpc_u_, mpc_u_, mpc_u_heading_);
+    3336           0 :   auto constraints            = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    3337             : 
+    3338           0 :   ROS_DEBUG_THROTTLE(throttle, "[MpcTracker]: MPC result: [%.2f, %.2f, %.2f, %.2f]", mpc_u(0), mpc_u(1), mpc_u(2), mpc_u_heading);
+    3339           0 :   ROS_DEBUG_THROTTLE(throttle, "[MpcTracker]: snap constraint: hor: %.2f, ver asc: %.2f, vert desc: %.2f, heading: %.2f]", constraints.horizontal_snap,
+    3340             :                      constraints.vertical_ascending_snap, constraints.vertical_descending_snap, constraints.heading_snap);
+    3341           0 : }
+    3342             : 
+    3343             : //}
+    3344             : 
+    3345             : /* getCurrentTrajectoryIdx() //{ */
+    3346             : 
+    3347       25225 : int MpcTracker::getCurrentTrajectoryIdx() {
+    3348             : 
+    3349       25225 :   auto trajectory_current_time = mrs_lib::get_mutexed(mutex_trajectory_tracking_states_, trajectory_current_time_);
+    3350       25225 :   auto trajectory_dt   = mrs_lib::get_mutexed(mutex_trajectory_tracking_states_, trajectory_dt_);
+    3351             : 
+    3352       25225 :   return floor(trajectory_current_time / trajectory_dt);
+    3353             : }
+    3354             : 
+    3355             : //}
+    3356             : 
+    3357             : /* increaseCurrentTrajectoryTime() //{ */
+    3358             : 
+    3359       15783 : void MpcTracker::increaseCurrentTrajectoryTime(const double dt) {
+    3360             : 
+    3361       15783 :   auto trajectory_current_time = mrs_lib::get_mutexed(mutex_trajectory_tracking_states_, trajectory_current_time_);
+    3362       15783 :   auto [trajectory_size, trajectory_dt] = mrs_lib::get_mutexed(mutex_des_trajectory_, trajectory_size_, trajectory_dt_);
+    3363             : 
+    3364       15783 :   trajectory_current_time += dt;
+    3365             : 
+    3366       15783 :   const double trajectory_duration = trajectory_size * trajectory_dt;
+    3367             : 
+    3368             :   // if the tracking idx hits the end of the trajectory
+    3369       15783 :   if (trajectory_current_time >= trajectory_duration) {
+    3370             : 
+    3371           5 :     if (trajectory_tracking_loop_) {
+    3372             : 
+    3373             :       // reset the idx
+    3374           0 :       trajectory_current_time -= trajectory_duration;
+    3375             : 
+    3376           0 :       ROS_INFO("[MpcTracker]: trajectory looped");
+    3377             : 
+    3378             :     } else {
+    3379             : 
+    3380           5 :       trajectory_tracking_in_progress_ = false;
+    3381             : 
+    3382           5 :       ROS_INFO("[MpcTracker]: done tracking trajectory, current time in trajectory=%f, trajectory_duration=%f", trajectory_current_time, trajectory_duration);
+    3383             :     }
+    3384             :   }
+    3385             : 
+    3386       15783 :   if (trajectory_tracking_in_progress_) {
+    3387       15778 :     mrs_lib::set_mutexed(mutex_trajectory_tracking_states_, trajectory_current_time, trajectory_current_time_);
+    3388             :   }
+    3389       15783 : }
+    3390             : 
+    3391             : //}
+    3392             : 
+    3393             : // --------------------------------------------------------------
+    3394             : // |                           timers                           |
+    3395             : // --------------------------------------------------------------
+    3396             : 
+    3397             : /* //{ timerDiagnostics() */
+    3398             : 
+    3399             : // published diagnostics in reguar intervals
+    3400       22909 : void MpcTracker::timerDiagnostics(const ros::TimerEvent& event) {
+    3401             : 
+    3402       22909 :   if (!is_initialized_)
+    3403           0 :     return;
+    3404             : 
+    3405       68727 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerDiagnostics", _diagnostics_rate_, 0.1, event);
+    3406       68727 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcTracker::timerDiagnostics", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3407             : 
+    3408       22909 :   publishDiagnostics();
+    3409             : }
+    3410             : 
+    3411             : //}
+    3412             : 
+    3413             : /* //{ timerMPC() */
+    3414             : 
+    3415       84480 : void MpcTracker::timerMPC(const ros::TimerEvent& event) {
+    3416             : 
+    3417       84480 :   if (odometry_reset_in_progress_) {
+    3418           0 :     ROS_ERROR("[MpcTracker]: mpc iteration tried run while reseting odometry");
+    3419           0 :     return;
+    3420             :   }
+    3421             : 
+    3422       84480 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    3423             : 
+    3424       84480 :   mrs_lib::AtomicScopeFlag unset_running(mpc_timer_running_);
+    3425             : 
+    3426       84480 :   bool started_with_invalid = mpc_result_invalid_;
+    3427             : 
+    3428       84480 :   if (!is_active_) {
+    3429           0 :     return;
+    3430             :   }
+    3431             : 
+    3432       84480 :   if (!is_initialized_) {
+    3433           0 :     return;
+    3434             :   }
+    3435             : 
+    3436      253440 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerMPC", 1.0 / dt1, 0.01, event);
+    3437      253440 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcTracker::timerMPC", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3438             : 
+    3439       84480 :   ros::Time     begin = ros::Time::now();
+    3440       84480 :   ros::Time     end;
+    3441       84480 :   ros::Duration interval;
+    3442             :   int           trajectory_id;
+    3443             : 
+    3444             :   // if we are tracking trajectory, copy the setpoint
+    3445       84480 :   if (trajectory_tracking_in_progress_) {
+    3446             : 
+    3447       31566 :     MatrixXd des_x_trajectory, des_y_trajectory, des_z_trajectory, des_heading_trajectory;
+    3448       31566 :     VectorXd des_x_whole_trajectory, des_y_whole_trajectory, des_z_whole_trajectory, des_heading_whole_trajectory;
+    3449             :     double   trajectory_dt;
+    3450             :     int      trajectory_size;
+    3451             :     {
+    3452       15783 :       std::scoped_lock lock(mutex_des_trajectory_, mutex_des_whole_trajectory_);
+    3453             : 
+    3454       15783 :       des_x_trajectory       = des_x_trajectory_;
+    3455       15783 :       des_y_trajectory       = des_y_trajectory_;
+    3456       15783 :       des_z_trajectory       = des_z_trajectory_;
+    3457       15783 :       des_heading_trajectory = des_heading_trajectory_;
+    3458             : 
+    3459       15783 :       des_x_whole_trajectory       = *des_x_whole_trajectory_;
+    3460       15783 :       des_y_whole_trajectory       = *des_y_whole_trajectory_;
+    3461       15783 :       des_z_whole_trajectory       = *des_z_whole_trajectory_;
+    3462       15783 :       des_heading_whole_trajectory = *des_heading_whole_trajectory_;
+    3463             : 
+    3464       15783 :       trajectory_size = trajectory_size_;
+    3465       15783 :       trajectory_dt   = trajectory_dt_;
+    3466             : 
+    3467       15783 :       trajectory_id = des_whole_trajectory_id_;
+    3468             :     }
+    3469             : 
+    3470             :     /* interpolate the trajectory points and fill in the desired_trajectory vector //{ */
+    3471             : 
+    3472       15783 :     const double dt_from_last_update = (event.current_real - event.last_real).toSec();
+    3473             : 
+    3474       15783 :     if (dt_from_last_update > 0.0 && dt_from_last_update < 1.0) {
+    3475       15783 :       increaseCurrentTrajectoryTime(dt_from_last_update);
+    3476             :     } else {
+    3477           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);
+    3478             :     }
+    3479             : 
+    3480       15783 :     auto trajectory_current_time = mrs_lib::get_mutexed(mutex_trajectory_tracking_states_, trajectory_current_time_);
+    3481             : 
+    3482      647103 :     for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    3483             : 
+    3484      631320 :       auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    3485             : 
+    3486      631320 :       double first_time = trajectory_current_time + dt1 + i * _dt2_;
+    3487             : 
+    3488      631320 :       int first_idx  = int(floor(first_time / trajectory_dt));
+    3489             : 
+    3490      631320 :       int second_idx = first_idx + 1;
+    3491             : 
+    3492      631320 :       double interp_coeff = std::fmod(first_time / trajectory_dt, 1.0);
+    3493             : 
+    3494      631320 :       if (trajectory_tracking_loop_) {
+    3495             : 
+    3496           0 :         if (second_idx >= trajectory_size) {
+    3497           0 :           second_idx = second_idx % trajectory_size;
+    3498             :         }
+    3499             : 
+    3500           0 :         if (first_idx >= trajectory_size) {
+    3501           0 :           first_idx = first_idx % trajectory_size;
+    3502             :         }
+    3503             : 
+    3504             :       } else {
+    3505             : 
+    3506      631320 :         if (second_idx >= trajectory_size) {
+    3507       88882 :           second_idx = trajectory_size - 1;
+    3508             :         }
+    3509             : 
+    3510      631320 :         if (first_idx >= trajectory_size) {
+    3511       81869 :           first_idx = trajectory_size - 1;
+    3512             :         }
+    3513             :       }
+    3514             : 
+    3515      631320 :       if (first_idx < 0 || first_idx > (trajectory_size-1) || second_idx < 0 || second_idx > (trajectory_size-1)) {
+    3516           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);
+    3517           0 :         continue;
+    3518             :       }
+    3519             : 
+    3520      631320 :       des_x_trajectory(i, 0) = (1.0 - interp_coeff) * des_x_whole_trajectory(first_idx) + interp_coeff * des_x_whole_trajectory(second_idx);
+    3521      631320 :       des_y_trajectory(i, 0) = (1.0 - interp_coeff) * des_y_whole_trajectory(first_idx) + interp_coeff * des_y_whole_trajectory(second_idx);
+    3522      631320 :       des_z_trajectory(i, 0) = (1.0 - interp_coeff) * des_z_whole_trajectory(first_idx) + interp_coeff * des_z_whole_trajectory(second_idx);
+    3523             : 
+    3524      631320 :       des_heading_trajectory(i, 0) = sradians::interp(des_heading_whole_trajectory(first_idx), des_heading_whole_trajectory(second_idx), interp_coeff);
+    3525             :     }
+    3526             : 
+    3527             :     {
+    3528       31566 :       std::scoped_lock lock(mutex_des_trajectory_);
+    3529             : 
+    3530       15783 :       des_x_trajectory_       = des_x_trajectory;
+    3531       15783 :       des_y_trajectory_       = des_y_trajectory;
+    3532       15783 :       des_z_trajectory_       = des_z_trajectory;
+    3533       15783 :       des_heading_trajectory_ = des_heading_trajectory;
+    3534             :     }
+    3535             : 
+    3536             :     //}
+    3537             : 
+    3538             :     // | ---------- publish the current trajectory point ---------- |
+    3539             : 
+    3540       15783 :     int trajectory_tracking_idx = getCurrentTrajectoryIdx();
+    3541             : 
+    3542       31566 :     geometry_msgs::PoseStamped debug_trajectory_point;
+    3543       15783 :     debug_trajectory_point.header.stamp    = ros::Time::now();
+    3544       15783 :     debug_trajectory_point.header.frame_id = uav_state_.header.frame_id;
+    3545             : 
+    3546       15783 :     debug_trajectory_point.pose.position.x = (*des_x_whole_trajectory_)(trajectory_tracking_idx);
+    3547       15783 :     debug_trajectory_point.pose.position.y = (*des_y_whole_trajectory_)(trajectory_tracking_idx);
+    3548       15783 :     debug_trajectory_point.pose.position.z = (*des_z_whole_trajectory_)(trajectory_tracking_idx);
+    3549             : 
+    3550       15783 :     debug_trajectory_point.pose.orientation = mrs_lib::AttitudeConverter(0, 0, (*des_heading_whole_trajectory_)(trajectory_tracking_idx));
+    3551             : 
+    3552       15783 :     ph_current_trajectory_point_.publish(debug_trajectory_point);
+    3553             : 
+    3554             :   } else {
+    3555             : 
+    3556       68697 :     std::scoped_lock lock(mutex_des_whole_trajectory_);
+    3557             : 
+    3558       68697 :     trajectory_id = des_whole_trajectory_id_;
+    3559             :   }
+    3560             : 
+    3561       84480 :   manageConstraints();
+    3562             : 
+    3563       84480 :   calculateMPC();
+    3564             : 
+    3565       84480 :   end      = ros::Time::now();
+    3566       84480 :   interval = end - begin;
+    3567             : 
+    3568             :   // | ------------------ calculate the MPC RTF ----------------- |
+    3569             : 
+    3570       84480 :   mpc_rtf_ = 0.99 * mpc_rtf_ + 0.01 * (interval.toSec()/dt1);
+    3571             : 
+    3572       84480 :   if (mpc_rtf_ >= 1.0) {
+    3573           0 :     ROS_WARN_THROTTLE(5.0, "[MpcTracker] MPC Real Time Factor (%.3f) is slow", mpc_rtf_);
+    3574             :   }
+    3575             : 
+    3576             :   /* publish predicted future //{ */
+    3577             : 
+    3578             :   {
+    3579      168960 :     geometry_msgs::PoseArray debug_trajectory_out;
+    3580       84480 :     debug_trajectory_out.header.stamp    = ros::Time::now();
+    3581       84480 :     debug_trajectory_out.header.frame_id = uav_state_.header.frame_id;
+    3582             : 
+    3583             :     {
+    3584      168960 :       std::scoped_lock lock(mutex_predicted_trajectory_);
+    3585             : 
+    3586     3463676 :       for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    3587             : 
+    3588     3379200 :         geometry_msgs::Pose newPose;
+    3589             : 
+    3590     3379200 :         newPose.position.x = predicted_trajectory_(i * MPC_N_STATES);
+    3591     3379200 :         newPose.position.y = predicted_trajectory_(i * MPC_N_STATES + 4);
+    3592     3379200 :         newPose.position.z = predicted_trajectory_(i * MPC_N_STATES + 8);
+    3593             : 
+    3594             :         try {
+    3595     3379200 :           newPose.orientation = mrs_lib::AttitudeConverter(0, 0, predicted_heading_trajectory_(i * MPC_N_STATES));
+    3596           0 :         } catch (...) {
+    3597           0 :           ROS_ERROR_THROTTLE(1.0, "[MpcTracker]: failed to fill orientation into debug print trajectory");
+    3598             :         }
+    3599             : 
+    3600     3379200 :         debug_trajectory_out.poses.push_back(newPose);
+    3601             :       }
+    3602             :     }
+    3603             : 
+    3604       84480 :     ph_predicted_trajectory_debugging_.publish(debug_trajectory_out);
+    3605             :   }
+    3606             : 
+    3607             :   //}
+    3608             : 
+    3609             :   /* publish full state prediction //{ */
+    3610             : 
+    3611             :   {
+    3612      168960 :     mrs_msgs::MpcPredictionFullState prediction_fs_out;
+    3613       84480 :     prediction_fs_out.header.stamp    = ros::Time::now();
+    3614       84480 :     prediction_fs_out.header.frame_id = uav_state_.header.frame_id;
+    3615             : 
+    3616       84480 :     ros::Time stamp = prediction_fs_out.header.stamp;
+    3617             : 
+    3618       84480 :     prediction_fs_out.input_id = trajectory_id;
+    3619             : 
+    3620             :     {
+    3621      168960 :       std::scoped_lock lock(mutex_predicted_trajectory_);
+    3622             : 
+    3623     3463676 :       for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    3624             : 
+    3625     3379200 :         if (i == 0) {
+    3626       84480 :           stamp += ros::Duration(0.01);
+    3627             :         } else {
+    3628     3294724 :           stamp += ros::Duration(0.2);
+    3629             :         }
+    3630             : 
+    3631     3379200 :         prediction_fs_out.stamps.push_back(stamp);
+    3632             : 
+    3633             :         {  // position
+    3634     3379200 :           geometry_msgs::Point point;
+    3635             : 
+    3636     3379200 :           point.x = predicted_trajectory_(i * MPC_N_STATES);
+    3637     3379200 :           point.y = predicted_trajectory_(i * MPC_N_STATES + 4);
+    3638     3379200 :           point.z = predicted_trajectory_(i * MPC_N_STATES + 8);
+    3639             : 
+    3640     3379200 :           prediction_fs_out.position.push_back(point);
+    3641             :         }
+    3642             : 
+    3643             :         {  // velocity
+    3644     3379200 :           geometry_msgs::Vector3 vector;
+    3645             : 
+    3646     3379200 :           vector.x = predicted_trajectory_(i * MPC_N_STATES + 1);
+    3647     3379200 :           vector.y = predicted_trajectory_(i * MPC_N_STATES + 5);
+    3648     3379200 :           vector.z = predicted_trajectory_(i * MPC_N_STATES + 9);
+    3649             : 
+    3650     3379200 :           prediction_fs_out.velocity.push_back(vector);
+    3651             :         }
+    3652             : 
+    3653             :         {  // acceleration
+    3654     3379200 :           geometry_msgs::Vector3 vector3;
+    3655             : 
+    3656     3379200 :           vector3.x = predicted_trajectory_(i * MPC_N_STATES + 2);
+    3657     3379200 :           vector3.y = predicted_trajectory_(i * MPC_N_STATES + 6);
+    3658     3379200 :           vector3.z = predicted_trajectory_(i * MPC_N_STATES + 10);
+    3659             : 
+    3660     3379200 :           prediction_fs_out.acceleration.push_back(vector3);
+    3661             :         }
+    3662             : 
+    3663             :         {  // jerk
+    3664     3379200 :           geometry_msgs::Vector3 vector3;
+    3665             : 
+    3666     3379200 :           vector3.x = predicted_trajectory_(i * MPC_N_STATES + 3);
+    3667     3379200 :           vector3.y = predicted_trajectory_(i * MPC_N_STATES + 7);
+    3668     3379200 :           vector3.z = predicted_trajectory_(i * MPC_N_STATES + 11);
+    3669             : 
+    3670     3379200 :           prediction_fs_out.jerk.push_back(vector3);
+    3671             :         }
+    3672             : 
+    3673             :         {
+    3674             :           // heading
+    3675             : 
+    3676     3379200 :           prediction_fs_out.heading.push_back(predicted_heading_trajectory_(i * MPC_N_STATES));
+    3677     3379200 :           prediction_fs_out.heading_rate.push_back(predicted_heading_trajectory_(i * MPC_N_STATES + 1));
+    3678     3379200 :           prediction_fs_out.heading_acceleration.push_back(predicted_heading_trajectory_(i * MPC_N_STATES + 2));
+    3679     3379200 :           prediction_fs_out.heading_jerk.push_back(predicted_heading_trajectory_(i * MPC_N_STATES + 3));
+    3680             :         }
+    3681             :       }
+    3682             :     }
+    3683             : 
+    3684             :     {
+    3685      168960 :       std::scoped_lock lock(mutex_prediction_full_state_);
+    3686       84480 :       prediction_full_state_ = prediction_fs_out;
+    3687             :     }
+    3688             :   }
+    3689             : 
+    3690             :   //}
+    3691             : 
+    3692       84480 :   mpc_computed_ = true;
+    3693             : 
+    3694       84480 :   if (started_with_invalid) {
+    3695             : 
+    3696           5 :     mpc_result_invalid_ = false;
+    3697             : 
+    3698           5 :     ROS_INFO("[MpcTracker]: calculated the first MPC result after invalidation");
+    3699             :   }
+    3700             : }
+    3701             : 
+    3702             : //}
+    3703             : 
+    3704             : /* timerVelocityTracking() //{ */
+    3705             : 
+    3706         821 : void MpcTracker::timerVelocityTracking(const ros::TimerEvent& event) {
+    3707             : 
+    3708         821 :   if (!is_initialized_) {
+    3709           3 :     return;
+    3710             :   }
+    3711             : 
+    3712         821 :   if (!velocity_tracking_active_) {
+    3713           0 :     return;
+    3714             :   }
+    3715             : 
+    3716        1642 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerVelocityTracking", int(30.0), 0.01, event);
+    3717             :   mrs_lib::ScopeTimer timer =
+    3718        1642 :       mrs_lib::ScopeTimer("MpcTracker::timerVelocityTracking", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3719             : 
+    3720             :   // stop the timer when timeout
+    3721         821 :   if ((ros::Time::now() - velocity_reference_time_).toSec() > 0.5) {
+    3722             : 
+    3723           3 :     ROS_WARN_THROTTLE(1.0, "[MpcTracker]: velocity reference timeouted, hovering");
+    3724           3 :     timer_velocity_tracking_.stop();
+    3725             : 
+    3726           3 :     toggleHover(true);
+    3727             : 
+    3728           3 :     velocity_tracking_active_ = false;
+    3729             : 
+    3730           3 :     return;
+    3731             :   }
+    3732             : 
+    3733        1636 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    3734         818 :   auto velocity_reference     = mrs_lib::get_mutexed(mutex_velocity_reference_, velocity_reference_);
+    3735             : 
+    3736        1636 :   mrs_msgs::TrajectoryReference trajectory;
+    3737             : 
+    3738         818 :   trajectory.fly_now         = true;
+    3739         818 :   trajectory.use_heading     = true;
+    3740         818 :   trajectory.dt              = 0.2;
+    3741         818 :   trajectory.header.stamp    = ros::Time::now();
+    3742         818 :   trajectory.header.frame_id = "";
+    3743             : 
+    3744         818 :   double x       = mpc_x(0, 0);
+    3745         818 :   double y       = mpc_x(4, 0);
+    3746         818 :   double z       = mpc_x(8, 0);
+    3747         818 :   double heading = mpc_x_heading(0, 0);
+    3748             : 
+    3749       41718 :   for (int i = 0; i < 50; i++) {
+    3750             : 
+    3751       40900 :     mrs_msgs::Reference reference;
+    3752       40900 :     reference.position.x = x;
+    3753       40900 :     reference.position.y = y;
+    3754       40900 :     reference.position.z = z;
+    3755       40900 :     reference.heading    = heading;
+    3756             : 
+    3757       40900 :     trajectory.points.push_back(reference);
+    3758             : 
+    3759       40900 :     x += velocity_reference.velocity.x * trajectory.dt;
+    3760       40900 :     y += velocity_reference.velocity.y * trajectory.dt;
+    3761       40900 :     z += velocity_reference.velocity.z * trajectory.dt;
+    3762             : 
+    3763       40900 :     if (velocity_reference.use_altitude) {
+    3764           0 :       z = velocity_reference.altitude;
+    3765             :     }
+    3766             : 
+    3767       40900 :     if (velocity_reference.use_heading_rate) {
+    3768       37550 :       heading += velocity_reference.heading_rate * trajectory.dt;
+    3769        3350 :     } else if (velocity_reference.use_heading) {
+    3770           0 :       heading = velocity_reference.heading;
+    3771             :     }
+    3772             :   }
+    3773             : 
+    3774        1636 :   auto [success, message, modified] = loadTrajectory(trajectory);
+    3775             : }
+    3776             : 
+    3777             : //}
+    3778             : 
+    3779             : /* //{ timerAvoidanceTrajectory() */
+    3780             : 
+    3781        4545 : void MpcTracker::timerAvoidanceTrajectory(const ros::TimerEvent& event) {
+    3782             : 
+    3783        4545 :   if (!is_active_) {
+    3784        2202 :     return;
+    3785             :   }
+    3786             : 
+    3787        2379 :   if (!is_initialized_) {
+    3788           0 :     return;
+    3789             :   }
+    3790             : 
+    3791        2379 :   if (!sh_estimation_diag_.hasMsg()) {
+    3792           0 :     return;
+    3793             :   } else {
+    3794             :     // we won't try to transform and publish the avoidance prediction if we cannot transform it
+    3795             : 
+    3796        2379 :     auto                     estimation_diag      = sh_estimation_diag_.getMsg();
+    3797        2379 :     std::vector<std::string> state_estimators = estimation_diag.get()->switchable_state_estimators;
+    3798             : 
+    3799        2379 :     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();
+    3800        2379 :     bool got_rtk_est = std::find(state_estimators.begin(), state_estimators.end(), "rtk") != state_estimators.end();
+    3801             : 
+    3802        2379 :     if (!got_gps_est && !got_rtk_est) {
+    3803          36 :       return;
+    3804             :     }
+    3805             :   }
+    3806             : 
+    3807        4686 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerAvoidanceTrajectory", _avoidance_trajectory_rate_, 0.1, event);
+    3808             :   mrs_lib::ScopeTimer timer =
+    3809        4686 :       mrs_lib::ScopeTimer("MpcTracker::timerAvoidanceTrajectory", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3810             : 
+    3811        2343 :   auto uav_state            = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    3812        2343 :   auto predicted_trajectory = mrs_lib::get_mutexed(mutex_predicted_trajectory_, predicted_trajectory_);
+    3813             : 
+    3814        2343 :   if (future_was_predicted_) {
+    3815             : 
+    3816        2342 :     mrs_msgs::FutureTrajectory avoidance_trajectory;
+    3817             : 
+    3818             :     // fill last trajectory with initial data
+    3819        2342 :     avoidance_trajectory.stamp               = ros::Time::now();
+    3820        2342 :     avoidance_trajectory.uav_name            = _uav_name_;
+    3821        2342 :     avoidance_trajectory.priority            = avoidance_this_uav_priority_;
+    3822        2342 :     avoidance_trajectory.collision_avoidance = collision_avoidance_enabled_ && (uav_state.estimator_horizontal == "lat_gps" || uav_state.estimator_horizontal == "lat_rtk");
+    3823        2342 :     avoidance_trajectory.points.clear();
+    3824        2342 :     avoidance_trajectory.stamp               = ros::Time::now();
+    3825        2342 :     avoidance_trajectory.uav_name            = _uav_name_;
+    3826        2342 :     avoidance_trajectory.priority            = avoidance_this_uav_priority_;
+    3827        2342 :     avoidance_trajectory.collision_avoidance = collision_avoidance_enabled_;
+    3828             : 
+    3829        4684 :     auto res = common_handlers_->transformer->getTransform(uav_state.header.frame_id, "utm_origin", ros::Time::now());
+    3830             : 
+    3831        2342 :     if (!res) {
+    3832             : 
+    3833           0 :       std::string message = "[MpcTracker]: can not transform predicted future to utm_origin";
+    3834           0 :       ROS_WARN_STREAM_ONCE(message);
+    3835           0 :       ROS_DEBUG_STREAM_THROTTLE(1.0, message);
+    3836           0 :       return;
+    3837             : 
+    3838             :     } else {
+    3839             : 
+    3840        4684 :       geometry_msgs::TransformStamped tf = res.value();
+    3841             : 
+    3842       96022 :       for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    3843             : 
+    3844             :         // original point
+    3845      187360 :         geometry_msgs::PoseStamped original_point;
+    3846             : 
+    3847       93680 :         original_point.header.stamp    = ros::Time::now();
+    3848       93680 :         original_point.header.frame_id = uav_state.header.frame_id;
+    3849             : 
+    3850       93680 :         original_point.pose.position.x = predicted_trajectory(i * MPC_N_STATES);
+    3851       93680 :         original_point.pose.position.y = predicted_trajectory(i * MPC_N_STATES + 4);
+    3852       93680 :         original_point.pose.position.z = predicted_trajectory(i * MPC_N_STATES + 8);
+    3853             : 
+    3854       93680 :         original_point.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    3855             : 
+    3856      187360 :         auto res = common_handlers_->transformer->transform(original_point, tf);
+    3857             : 
+    3858       93680 :         if (res) {
+    3859             : 
+    3860       93680 :           mrs_msgs::FuturePoint new_point;
+    3861             : 
+    3862       93680 :           new_point.x = res.value().pose.position.x;
+    3863       93680 :           new_point.y = res.value().pose.position.y;
+    3864       93680 :           new_point.z = res.value().pose.position.z;
+    3865             : 
+    3866       93680 :           avoidance_trajectory.points.push_back(new_point);
+    3867             : 
+    3868             :         } else {
+    3869             : 
+    3870           0 :           std::string message = "[MpcTracker]: can not transform a point of a future trajectory";
+    3871           0 :           ROS_WARN_STREAM_ONCE(message);
+    3872           0 :           ROS_DEBUG_STREAM_THROTTLE(1.0, message);
+    3873             :         }
+    3874             :       }
+    3875             :     }
+    3876             : 
+    3877        2342 :     ph_avoidance_trajectory_.publish(avoidance_trajectory);
+    3878             :   }
+    3879             : }
+    3880             : 
+    3881             : //}
+    3882             : 
+    3883             : /* timerHover() //{ */
+    3884             : 
+    3885         469 : void MpcTracker::timerHover(const ros::TimerEvent& event) {
+    3886             : 
+    3887         938 :   MatrixXd mpc_x = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    3888             : 
+    3889        1407 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerHover", 10, 0.01, event);
+    3890        1407 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcTracker::timerHover", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3891             : 
+    3892         469 :   setRelativeGoal(0, 0, 0, 0, false);
+    3893             : 
+    3894         469 :   if (fabs(mpc_x(1, 0)) < 0.1 && fabs(mpc_x(5, 0)) < 0.1 && fabs(mpc_x(9, 0)) < 0.1) {
+    3895             : 
+    3896          77 :     toggleHover(false);
+    3897             : 
+    3898          77 :     ROS_INFO("[MpcTracker]: timerHover: speed is low, stopping hover timer");
+    3899             :   }
+    3900         469 : }
+    3901             : 
+    3902             : //}
+    3903             : 
+    3904             : }  // namespace mpc_tracker
+    3905             : 
+    3906             : }  // namespace mrs_uav_trackers
+    3907             : 
+    3908             : #include <pluginlib/class_list_macros.h>
+    3909         109 : 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..7fda2a1cc8 --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.overview.html @@ -0,0 +1,998 @@ + + + + + + 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 + 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..097ef4ceac9111f66044b3390a16e99fc7427e5d GIT binary patch literal 12562 zcmV+tG40NYP)%odUzBwJ{AFx~%FZWDiipkTD=oLk?!J=2z=AR;2*<2gN_&-wd* zk6!H?--+~DLzw`ppysYao3(&fhd?k2{jyzO>o~_K_RUbO2ISGQfE>~*7bth_QH@axs2!u0m=fW-1~Fd8Mlo6$E(5oAtp>c*@i)dxf6Zc? z&OuK75Fa!;QY%OSVE3*oSk-`SU3cN;n7Qsc)elef+-+HVj8omLPy-sfo~NoZ^;CE4 zJ0s=-E|0Ol^gt0W0e0^hbX~zcpzF!4nsWqwFnUss*#=7J`O0J=^)gqzT*jmN6GF zhIu}>>?+{X@9jOhMmFX|CM2^MG#IfYB!OT7nZIi%i|v}5C8_K78O=JC=8_rR#ks^Y zR`yhOJj}2d3!Wj}80}rpz0QVi(iFJdq_xWmi?P#s1{j~|!_%JxChv?X0#5G!;QY!0 z9zRuK41fiUWGV0)aTJ+u(wk=RIFHXyIL0zyB3i=u{f!Qz2b3`CiJ5}2Fo%<$N131Z zfMs5BUylSVz=Fkfd6qrK$exqeQ#fn;>A_uh_DaLEz57D?1YJjppM^2}ARvWh*Y3%) zDVOjk2iPv*GumcKTU_pxm@XY#!0c531s-PGd&E>DDRL|$=?XEk6Uy@8^LsoK92uQr zIK~N5Fl9<70;1(F8$B(6p=$wf#>W3QF;h^w@gl)?`k0ja_1UFHwLiV_QJvl((pSWQ zV0631!Gp4|9TSrd3o_vOA>jWxVrIqM#D#|z7Gq52fG6$?R%cKtRTPMi&jL{tPy~hy zuOU?uLz=jO(x2!K4wkL~Nf`5Wz}&5<1T+&94$Rk2HE!qYcypFH3&{P#WoH4$#~T5k zM$Vmo(ba&AL#c6=5?2T8F+7KH0$!uIrB2}|Z9;Ks@qW%`D7X{;(mKX;9aRBj)h+XP zd=QCdD=Um~cBV}Q#WSk3xqJngR!8fY7>tzciQ$6*h}da z6(6%&#(2S*I>rB`GxHP<&Hynph0UGDuA?RDj_k=tf(T-|zDPFP_52J6C-?6O$X&&C zrFurh2S?G&b$zK{+_l%y4b2P z5cg?vQpuiYF4}(KfHz0CRoL_>V73SuDVa<2ABgH}6rUIo)p6Bar8<5n-XX!ic*+_eqgIkw2IEz)D|KjK4qfoXCBFu4vldW` zG1oRc;8pg78(1_;4eXh&4XKQ=bRkC+PxbE3(Df9oEA|sVMh2L%anX4>{%aZ|5sDch zZL@1aPzR{R=sgeiQ4x$|baW!u@42_wg5hVf5!1AKCx4*`(k%mQ^|Ei< zzAe@EsDlK+Fo-3RQ32c{AUhCi*DK-~hq8efnT}SBfog&IdW?^nGE3$)&}dPWf&~Bo zl^E5{FGIikr+c5-55kNQ%nV*UZwPB1>L*q37>bqVs zl8x6=nc#k=Z@z{1^YuD@a4WRVnK=O$E#U6vvBMN5s<+~T zz<_Fuy9|5lhNJ>q@7**tfCJV{e?E2}OR{Tz!P^aLI1*z$Q?_a6Yv}eH4Kuo%!;>&E+#2Nh zs3j)dor2;8S1#U`Ocr?c$E|A(;HJi?1l&qv)bf#82phB2@y0Nx&g?fn)!74GpEPH^ z`f<=P=EIACP3+-#9RZD0hc9x>Y{qb5H@+~}>n^Oq$m*lX9vWa%ctG@rQrB=^M8Gi9 z;>9BEx(etqO6n3@d(0ZxBA~KCC6z+FI6=?Og)4Vp**4Rrd00Z@MdRsA%r?rLQJh{+N2S)B=^p6<58|a! znpR=w$SbloMKeUt}O9=Qx> zt|2cC_+wlzj98qol)=^B;@c~qb=y4bG(f2h=PO2qeER>O6CS$aiQE1RA$hq^ zuLP)c#Rum;K*xIV2ZfuQJY>}P2G*KIrNH?4E&{1lLrGiLYznO}<>QPDdR5sTq$i1w z|G>|+PI~SXil`t z=hs(Zm}bE5eX0t0*ij0qM~vpxb1bo-WNH+64y+jhL*(&SS7mkE{K-Cw9#N>bgD)C}~pols(_7dkR89 z?}n8cw}f%SfFmwwQ_saQssVNFk$aHOsD~|nyIV18rOeW(dHsw56K!|O{B|v}CzFQ~ zpo-xtKqbTfNn&a_ivnN{Y1z5dfF@!n)75}ELBCK_Wcey1qDZtp9zb=0-DawfPsOwb z$etjO;w5r-_ORFc?A5ul9WIhh<{kp1J@dI0wu|oB|Mvv7{`}PeAlnsXoPD*4Q-uC*@?@g2$t$Bw_yZ75tY4qR{$^(V}~*B)#!c8p+Y%@oCUxwwMM(AU>pwK z*!x{e)9LPpvFr|0s}yzD169hiQ>CWI*B9|*3QkMIO>V+ibRsdO2#(~g-<9|kiK+N< zi^RM#z`Ka?H?x`Z`YUifj8WV5eHYz{;RV0v*Wvoq^3ih9oiQ#XE`EWF?^}z%XlDcX zQX@?Q)#@>_x6K!lD{_YW4AJCIf7!;!-{a)7007HVGz3uB{!L;&>etc{P=oQjD(D8N zYfwpn*%4=^&dVWyW@4hoV0Bv~#+Rsi1{KG;%Z-zW{?=Wm`g$E7vW>!Ga((%-O%*^T z8x4FMX5;pY<$UaULr!d6%rNjv=(`4~JL!Z*2eaUVcsQuAVdbWSj~tc;0voNaPxlOm zj%vze;?*QT!Xge4Ip;Ix>1J@z2pu`=gT)}mY>!idp1_s-gM zoysLFk=LHx4tZWp!hkW&;6_=R<{yP1lni_b81}fAI}^Ag!|UaY*%Q!N2;~z93cQf) zi2$ezyl^21pnOxHoJc+jK`42qxx}yS{O7#6-_At+P$mj9SH^f-n%2X;s}(Qw(|cQU zI@}!f?4$5h!a2fHs1q~zE^h6mDdI%OB5Nph(u#4(#(0X)7QS7bB((%USEK`rBhD`( zK{iNi#oz|>82y2{VErtaL!9P8abxTc=$XRZ-ZOwZd(Gd%p1D$3rAkap=NJlj3w!3O zD~@!%NEZg<0mZut6V*C*;R5>JiiwDL=MqCx^c#{gpfWQRL?;9R49PRe-F*J@{dGY6 zV-m9}!>iLA_}Cv%*V?4#kZ`H2RBp})BONY1r(G)KO5Gb zCDnh#n2F(>Ff1_wU<)y4LyD>o#)Ug-@_V5Uus6o@r!~O%Q)Soe5pBT6u5Lnb$Y->+ zeRznuG=>6jSZRz}Et>~hf{odhI2)JUy}&W0=wSa9J6GjQ(YCHV#uQXe(gx|$QUMpnUccRT15c|#t4B6L= z|5Fg6a?Vvu=v@)u#`(BCB=MyLz%vRX#SU!EF9T&gCw4YmOV0)pIPCC<5u97bjqElVK~Ok@f~t1YQ)hoaa*Zo0c2aL>jc;{ zt(x2T>CfIp0i^QUT_XKe3Se)y!{w=@Df9(EL)WKf$PIwaFH``SRtHPHz3Xb|e889_ z&@J3`4-uMc_~CY@N~nvd$8cKrgvcE7XO8YXVND6H z`aE_tWm}jxmf`9t1d=absoD$WImC3tJ9?d<~3j$+X?6W|iFGm3Xe%4RNxu6PnvYg1gy5yw$>=f?q0#p!!AX?uWR}gNyTnMt(NvvMI)0`4nC$@@ij+ z`Z{R7#9^K(4S#)Lk!I%tw~w_u{_41jt?$Gk_lU3u_KjVQ0hIR?1@@ zZ^DP0sO51KL4bp>wRbHt477q3@D}=W?|o}_qi~Dy~Dmky#z&N7^U=Z zQ%JcKX2ua&nZy9NWwRa9jR_wufMvhpJ25<`I=gnU06M>cn>TmP^cx$9p4N}i&;WbN z46?GPd_Y>vfYpN5gQaJVY+|WSYi%ufN--dN#T@|0ID7|qxpa<9we!kAOyuP59W*(C zZFic#xl%0Xc-w_z*Z`(t6m}CJ%cJ(2TKqX-AXv`gQHGRsc0uLrJnbz9Ras zNqBPCU+zJFholxW!_36O7MYi=6{A%itI26vK+72aRe9`isX63MQxr3rV630QKQWVq zVz}D7E@wY0W{sI7e_~b?ios&m!;Jhhlr{Qwt5b;7R z`p{h)eFMo##A>Aa5lh{H)Oj`t#?7IR*MVw-0(N*JqcL1LTo}$_oVf09%352Lu8&6N zKGssr#Pp86T&N%CY-ew1zyjb2tOeZafSxZD@U|eEB#!0!9Zma8@!7W4yWw5TO1zSX zbYjiM+UC&M`Fo~@F_x}yqV&AiUf8SjwXhcwj<;`Iii!9b??``MkK1rT&T&-pHP~ zy`~QrF|w={0WW74;8SiPUt#d29T4HHYbc6EVzA8iNxmr96iF|=p=uuGltxn-*aL%c zN`Ip7dhoT&i8&jo9izY(32-Ua8h|Ji2RNuq>C)uJHc{DJQXsYlZ@5!PF5@ zxwwAAa*g6of?UlEXNw1VS%5R!Y+eh_uqy_;bm24vuyKreH1cMzNM2j7tFQB+$GFzA znmk~3P7&GD9gs;S;KrzKAG}V1O6AOH%d7YcI)wq`b&4tFM5R;iEV$X}ne{#D0_z=; zcnijL&8?P{YOFtB4+wQ19+z@GXh57g^5X`R>*owj^Sx`nyPjV&%g|<<1#7JIZW6K| zXD8}YI4q$*R5}YSW9+f6+q#x!kQz1m<*GgntH<~g%k}Exo2+e6u3|JI|wK?pnDe%3;9!f!1MPcL|vD-5VH>XY(; zoH?UWdfZ7T6?a}=fu~d9lyJl-$4G499-1Ogfo6v}fRB1jk!0e*i$jBSjcQ`OFB)fBE;MzXN}RaFW(;rKqbZ@RWBXKs$=|zodsR4r8(7{%jBOt zmfN2^_7O>c3+I)145E)3-QSQsAG^lQwQVi?b%qyWZ1hPLGg(l0sF;zzG#VdF5J|c) zGGW}to!&P&fYrZ`o^#=$u2;GfC0T5lUxqpnq~c z3i_uE2(EyJDJituvzO1=%t%F`#>`A_IBh#9Rxx~VAbI5)P}j8(Pm$S5cH<)kd3k}0 zFO_*4l!o}b3TupZwx4J4vO{#)G2*0EA$ZXe%Y_`dsX%Au$G!6vQDUo0#JpLUZM24g zeRMUg zyM830s&M>9Ky8Cs57k19$B}5dLOvz~H=mgSd!go>X;t>eNnG;=1QMH7$mnbbJU{x1 zqqN+X7El>7$c#~ZtFjFcj4{m9SpnDr*t>ctYpyfwvUxtzT3C=!x*xHCiCn0IHY=ch zjG4-nsHSidM%C2Z*|nv4=Y?2IHb_rKj7asOfb(-5pDNu%x*3eu(=XqFm=OxVrtJMd zLmlXi4A=r_Rm}+F9{C!6U;(`SWuRa;$D4;{Lwo_sai<>ugN2~jE&HzJF0L#b)edfcN2$X5(ng6}N6UKT*@NmJWG&#BAfp!-eSXZ~V?uIrB&>-H)>)03!veP!Jd zz~6Je&IR(6sy8`bgTr)}1IW&?XG0u1qdBZc(%S2V=0k4RWAm-YJ;ZV8Fl@XfT_Y|z zJ>G)x&f}gIqkNue#~5w1n!`8mJ3nroRWyRcgLHo=)=vz8LGR>%{(f%_+<8Nsgzuv8Y%uw1q0p8Vxh z?`8XPX~UtChdvwdy`tj#6(knI>YAreQlpnJ_K(m&Efey?&tl9?saq*-&8V#u546Y_ zSBQ}<(hhjl97Q63&|7=b=6azRIw&0ilpwv60!hRjNZMS_pm)xeb(R!`sp*s41N7b7B@ZiO_!}r6JtqK?+$PA$vvi z{A8Aba0VE#4Xc}OjH3>adkWaW-Ql^u4v2v(fExhR&J4a+knr^aguqom7~f+gMlAf+ z4Uhnr0ow{3{%gcwEPV1{vUre-(y14$!c zFTG=^TjdFHhwK{)*_Vp5b zKI#MfV4_G|1t0&_pCgZN^+g!nb%9IaC^@k7W9*M{(9Faago;aHiHo!-;T~{HCz-2v zNJ0`~##PH0dpliX%JS^O#A2$3EA=;ry3r3XMA))T*#J29ULkaOF}OI_o077pBEvGk z0N9;9!LW~({W0@AANO6#CBal5KS;OnB|a*N2{y&qb4mA%1FX42Eev>T!=ACzR*!UHq3TP`Ia%#wkh8&-8P4^As}?OC2;rj z@-cFg&$ZApSCeW6OBkt28FCP(CrGBEgZ-Go<^Oa6EXD1q0gc2kb5R78|3CoD#fJk7 z7G3)6oznCaBj$7IsJ`(HT6rl@z(Gr)=o2y$^KdEVvGm9kwca#CEf}L`>w~;`X0m{T zJVtGp>FVx(Qp+t^VqS;TefhdT0XQypCPj`j#)>77{BJKTv&S$qZ?+e90W=cB%taAU z{sU<*Yl+WCFb~EQ7Pb+qnV42u(z69$p73F{-k$asrnvg*Xpy4)DW=Ih3x>KVm#YpF4I;%=r(yn=}(sFA_0|n7^6%o0-3vsS%@Yi!w0|Q(YJZ zWlxFf_k3EGWlxcL7;sNf7v&HHvZ)KeL*tr}+XY0lhpy8lz~*h|I9m8!Zc{`kpBl5C z5gAG=<@+$GBV^JhGo64#GUF3}Q)33!=D8<8leFxLuTi7K>RnR zVnpF~PX!n|38bIjw9bq+u_2DJ8VWonIxLk5?@`{M?YIZTpg1#bdO(0$5imRdSf}vb zjBbse%Y<=(hzjTHCXDjobr7I^jD_QHD@JjuPrw^10+kvV;{cVSB<4+$pyPdAAB>rT z*qnS{@b~QfQo{XUH1pnOkuqlw&D9Pa>@xWK7&>XXkyPA7_zR;ft{VrAz00@_-nA`EFrFpTi{9%GrJws>qq_f1#X_9Y9L)mN4AEa1)jZ9=XRiN0R6oK4 zv{?hmcK)FEe9#wq=ja~y3f7(G{gIwL{!OqZjMG#t0Q(Ko*$45bA3I3TAJzXKP4WMv z`jPxNra#j0@&CDI{-0E*<4eA1+SiP)1e7hBKS@2P)&Kwylu1NERI2aM6g{n!t83=u z&ny1m>-BbP=Ko1`!T5neu1w@n z2AK4Y1Z;P6@2n#g0rsx8qjWVJh#C5>y>ksrF~F`p(Qb+eJB<;h?iJo`!cx1T{3!UF zJ3|mo8zuT}3JhAjddhv=87@sgZYuJ>ccV&#$5 z_~d7$1|)tqAw|S{^XD^ue1yuAZbR+VHLO&6d=A~7f%wOBrM}?i7jEg@(@(fRIlxZ@ zb$nsEW}cB{#~2juscwt6DffkFsoTQUd|`=EHvIlvs@8r%S(1A8XB6q?%JK%$UtMr9 z0XzfI6QI|a+fJXB-4=*3l>aqTjo)czeAC~1Q3}zj*=Fjl7f`qm#a=dJjGNqa78;Fv z6Ein41bOxW#__anG%b@I3hcpe@Jk(Vr<_Gc-$JAU1j4VBY8`vdHRFx07c%kOA`BK) z?gJ2vt^imyy7cS%F(4RNm>RC`VLYIfbiqQWCe94r=}l{fyyh!N#Rr(07j{E6G8|%z zf&ujM*HOx(*We-H zG$`>hpikMbZ6EQ0C8#B4&iRvkO*Qrar&MveF`%X#Ypp>&H_y>0cEMlaOqePCi_shoxHI;LEj*OT1!V2G)=*%M6gSG@fE;rI;2WrvblB?^`L(#Y-D^3^ z=6o*IbM-om(K7o)W0`-aZ=?Zdsh6wIg@tX=!g;xBUQ4A;#|Oc9-9(f?WY z_vqhF)pflH9wcgAj0Z%p1hDswxW4`1QlPKpwjEPS;~fmx&{*peInBy0JJK^>FlikPxC0+7o7J+J`P6Vb*jI~3E_#-O;PtEs;{ZTS%Xin6 zH)tL@E}Faw_Z2QnG`m6bhHGZy%1_%Sz%5}c1A6E>;DX0ktY=@GnRjpuLGkwVhR@+P zaJ8MeNWo2LbA-Ej>xr^|k=i>dbDqs&#Qm?p5I)qpT;UYs_0xp0ynVdWD(trs`Ot%Wfti;=lwOKH{MNi&SC^P2hp3GJ zG>#D}0Jelcg950RYWYg{Ld8eh-d+J*#Q0ArfJT?H)G}M^axUp1xM$G`6vWgovBBM7 zd;Qexr%3oJfDIT+k^TmZp?bkeUaY@l<8}9J`!R)&y{rOCx6%ioHd0#K z^a?=P4vZ`D^JuQlL7J%l^QNw;ujwK&{qsIRRt(7Eex+n?U#BputR!T`G^A;P^cmJh z)tj#G5s!~m^YxeLu$qvr*w#PQT*nBRC&VCIbq|Yi!-lqH`%nt`0gVNN=hn5`6mI_* z5X-eFm3Y3z2}-9WXZ4DxbHDRm)>M^ddd`k%6taM%46o&XIv>1o#9-_fd zokBE)O7{$LAuA3@Z*v*n)#aknJ=XG=BX!Sjw(L7=vzw#r@YvM2Be(uGyXqu<9MSol;g$~y8jH#UKl5$?Vm{fUwc5H(-Q(*h}OmAU9b=qDT{q` z71PE6?rda11T+vs|M+Ov*ppfzES_{t^MJ2)?L!bmzy@N9DBBqhPy?s zR$x$?Ji`xAWj`^8&e($b&J=0Q7;d z>-nM$oM`!cmYeJ>R65j@w^ z<9amu4EWbqyuL%lc?@&?m99+L(!l<=CXDp&zhc zbd8Z7R(?iEPXKa%$m**DppTgO*Rns0LJ@tCzSJ`4I%g9c@Ow#5zZ)Osz}n4a=gla- z0YJvrf&dL&Q{yCJMu6{kyVqm7g$&x==e@*fA3E2iT&-4TDyUn*02(l&>_1p7&i7-) z82ux@I^@jOL4MF90JO02T5OI46isrIzbBM+E{oX*d9f+!U94hT-vt1{1Fbix}hW z4*EBTeht?ZGQ&^)-%h19MU+ zd~d)Q6D0uIO+jj{M72^iz+RT|=m zF+RGx3zsz6rMF8$s2frytn8XX-K=9QzFh*wSbn?Y_HaC}qC!|0*pY&2SU38bR!mp$ z^63xm{$eqAYV(4t4PY@4ll1nwM#ht4)fhlkqZ|7sZH7Y(q~B2T24xqNEc^a|bl&J$ zVSq(CD&J*(#s?I=&FhU`YllOMZNRp&=ommZBg``={3QulHqG5VE*)>0I}-1 zJ$WyT(akm3uL;x4{_wSS%6@RiYoGJ)qt4E=#^(TEb9npe`3$cpTltnXdwLOBvbj*? z%enPQE8`UYvX@vmM}-L^x-sbYf=JG3CcdW#=pPBO0jkD$G?hA7Fn{vh(tmQN;d5Pb zr)qpmDV={gJT?z`)#z?8#f-27-DZEcG5Tq~G?X|6L- zU=Kf>#%1%Ht(FvbP1#<{4wEchjs2gMy&@4Qs67-RDFSLP3^8xE#R60d5wUUG1gcyIN;P8#C-|YYoQX6)&WL{`$D)Jx}K-F}%2W zMC7J>1$T`wKg)Y;ss56yc#_7b>pHWyytszvSC zr#jnpIT1Re>_`C;>7Hvq*f(PtZQ&PUH`sUBBlaQEv~ zC_npF*{S_9ruid4o~7Dw1gNDp{*EKSFHUXD7V70Ovhnxa7=7K@IfT1RDdEQlMru5x zfS3jDlYy`$v-^Zdm)wrD?OZ} zZ@DwY8pp`x_GBYy^qZAy&uTG>5x%V${gFK;?)fkbe!1>hfB7jSGYdBeSRe)if{uhp z7Xs+wadR*YAAcP&yHzNd{JE@Il~k<;t7VKIK9cJNjl)k+e!Q18;-V@12d^r-RT<9x6jB*2qjP`d4ly|1zu~%|ctYBgHP~z# zH~!h`Uv9Y^QJL=5UDRDv-z7Lb>HI;;EsA1(um#h9o}Z2$lO07*qoM6N<$f*ZGA*8l(j literal 0 HcmV?d00001 diff --git a/mrs_uav_trackers/src/speed_tracker/index-detail-sort-f.html b/mrs_uav_trackers/src/speed_tracker/index-detail-sort-f.html new file mode 100644 index 0000000000..1a63932132 --- /dev/null +++ b/mrs_uav_trackers/src/speed_tracker/index-detail-sort-f.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:6444814.3 %
Date:2024-12-17 22:35:29Functions: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.3%14.3%
+ 		14.3 %64 / 44836.8 %7 / 19
<unnamed>14.3 %64 / 44836.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..9c71ab0585 --- /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:6444814.3 %
Date:2024-12-17 22:35:29Functions: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.3%14.3%
+ 		14.3 %64 / 44836.8 %7 / 19
<unnamed>14.3 %64 / 44836.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..8fbe78e7ec --- /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:6444814.3 %
Date:2024-12-17 22:35:29Functions: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.3%14.3%
+ 		14.3 %64 / 44836.8 %7 / 19
<unnamed>14.3 %64 / 44836.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..b5261e368d --- /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:6444814.3 %
Date:2024-12-17 22:35:29Functions: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.3%14.3%
+ 		14.3 %64 / 44836.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..138c7d8d76 --- /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:6444814.3 %
Date:2024-12-17 22:35:29Functions: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.3%14.3%
+ 		14.3 %64 / 44836.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..06423e75c0 --- /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:6444814.3 %
Date:2024-12-17 22:35:29Functions: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.3%14.3%
+ 		14.3 %64 / 44836.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..78a5d643fb --- /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:6444814.3 %
Date:2024-12-17 22:35:29Functions: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()109
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>)109
mrs_uav_trackers::speed_tracker::SpeedTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)312
mrs_uav_trackers::speed_tracker::SpeedTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)387
mrs_uav_trackers::speed_tracker::SpeedTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)142109
+
+
+ + + +
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..275990837d --- /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:6444814.3 %
Date:2024-12-17 22:35:29Functions:71936.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()109
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>)109
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&)387
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&)312
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&)142109
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..014a500299 --- /dev/null +++ b/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.html @@ -0,0 +1,1259 @@ + + + + + + + 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:6444814.3 %
Date:2024-12-17 22:35:29Functions: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             : #include <mrs_lib/service_client_handler.h>
+      18             : 
+      19             : #include <visualization_msgs/Marker.h>
+      20             : #include <visualization_msgs/MarkerArray.h>
+      21             : 
+      22             : #include <mrs_msgs/String.h>
+      23             : 
+      24             : //}
+      25             : 
+      26             : /* defines //{ */
+      27             : 
+      28             : #define STOP_THR 1e-3
+      29             : 
+      30             : //}
+      31             : 
+      32             : /* using //{ */
+      33             : 
+      34             : using vec2_t = mrs_lib::geometry::vec_t<2>;
+      35             : using vec3_t = mrs_lib::geometry::vec_t<3>;
+      36             : 
+      37             : using radians  = mrs_lib::geometry::radians;
+      38             : using sradians = mrs_lib::geometry::sradians;
+      39             : 
+      40             : //}
+      41             : 
+      42             : namespace mrs_uav_trackers
+      43             : {
+      44             : 
+      45             : namespace speed_tracker
+      46             : {
+      47             : 
+      48             : /* //{ class SpeedTracker */
+      49             : 
+      50             : class SpeedTracker : public mrs_uav_managers::Tracker {
+      51             : public:
+      52             :   bool initialize(const ros::NodeHandle &nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+      53             :                   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers);
+      54             : 
+      55             :   std::tuple<bool, std::string> activate(const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd);
+      56             :   void                          deactivate(void);
+      57             :   bool                          resetStatic(void);
+      58             : 
+      59             :   std::optional<mrs_msgs::TrackerCommand>   update(const mrs_msgs::UavState &uav_state, const mrs_uav_managers::Controller::ControlOutput &last_control_output);
+      60             :   const mrs_msgs::TrackerStatus             getStatus();
+      61             :   const std_srvs::SetBoolResponse::ConstPtr enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd);
+      62             :   const std_srvs::TriggerResponse::ConstPtr switchOdometrySource(const mrs_msgs::UavState &new_uav_state);
+      63             : 
+      64             :   const mrs_msgs::ReferenceSrvResponse::ConstPtr           setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr &cmd);
+      65             :   const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr   setVelocityReference(const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr &cmd);
+      66             :   const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr setTrajectoryReference(const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd);
+      67             : 
+      68             :   const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd);
+      69             : 
+      70             :   const std_srvs::TriggerResponse::ConstPtr hover(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      71             :   const std_srvs::TriggerResponse::ConstPtr startTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      72             :   const std_srvs::TriggerResponse::ConstPtr stopTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      73             :   const std_srvs::TriggerResponse::ConstPtr resumeTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      74             :   const std_srvs::TriggerResponse::ConstPtr gotoTrajectoryStart(const std_srvs::TriggerRequest::ConstPtr &cmd);
+      75             : 
+      76             : private:
+      77             :   ros::NodeHandle nh_;
+      78             : 
+      79             :   bool callbacks_enabled_ = true;
+      80             : 
+      81             :   std::string _uav_name_;
+      82             : 
+      83             :   std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>  common_handlers_;
+      84             :   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers_;
+      85             : 
+      86             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray> ph_rviz_marker_;
+      87             : 
+      88             :   // | ------------------------ uav state ----------------------- |
+      89             : 
+      90             :   mrs_msgs::UavState uav_state_;
+      91             :   bool               got_uav_state_ = false;
+      92             :   std::mutex         mutex_uav_state_;
+      93             : 
+      94             :   // | ------------------- tracker constraints ------------------ |
+      95             : 
+      96             :   mrs_msgs::DynamicsConstraints constraints_;
+      97             :   std::mutex                    mutex_constraints_;
+      98             : 
+      99             :   // | ---------------- the tracker's inner state --------------- |
+     100             : 
+     101             :   std::atomic<bool> is_initialized_  = false;
+     102             :   std::atomic<bool> is_active_       = false;
+     103             :   std::atomic<bool> first_iteration_ = true;
+     104             : 
+     105             :   double _external_command_timeout_;
+     106             : 
+     107             :   mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand> sh_command_;
+     108             : 
+     109             :   void callbackCommand(const mrs_msgs::SpeedTrackerCommand::ConstPtr msg);
+     110             : 
+     111             :   // stores the post-processed and transformed command
+     112             :   mrs_msgs::SpeedTrackerCommand command_;
+     113             :   std::mutex                    mutex_command_;
+     114             :   ros::Time                     last_command_time_;
+     115             :   std::atomic<bool>             getting_cmd_ = false;
+     116             : 
+     117             :   // | ------------------------ profiler ------------------------ |
+     118             : 
+     119             :   mrs_lib::Profiler profiler_;
+     120             :   bool              _profiler_enabled_ = false;
+     121             : 
+     122             :   // | --------------------- service clients -------------------- |
+     123             : 
+     124             :   mrs_lib::ServiceClientHandler<mrs_msgs::String> sch_switch_tracker_;
+     125             : 
+     126             :   std::string _backup_tracker_;
+     127             : 
+     128             :   std::future<mrs_msgs::String> switch_tracker_future_;
+     129             : 
+     130             :   std::atomic<bool> switch_tracker_called_ = false;
+     131             : };
+     132             : 
+     133             : //}
+     134             : 
+     135             : // | -------------- tracker's interface routines -------------- |
+     136             : 
+     137             : /* //{ initialize() */
+     138             : 
+     139         109 : bool SpeedTracker::initialize(const ros::NodeHandle &nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+     140             :                               std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers) {
+     141             : 
+     142         109 :   this->common_handlers_  = common_handlers;
+     143         109 :   this->private_handlers_ = private_handlers;
+     144             : 
+     145         109 :   _uav_name_ = common_handlers->uav_name;
+     146             : 
+     147         109 :   nh_ = nh;
+     148             : 
+     149         109 :   ros::Time::waitForValid();
+     150             : 
+     151             :   // --------------------------------------------------------------
+     152             :   // |                     loading parameters                     |
+     153             :   // --------------------------------------------------------------
+     154             : 
+     155             :   // | ---------------- load parent's parameters ---------------- |
+     156             : 
+     157         218 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     158             : 
+     159         109 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     160             : 
+     161         109 :   if (!param_loader_parent.loadedSuccessfully()) {
+     162           0 :     ROS_ERROR("[SpeedTracker]: Could not load all parameters!");
+     163           0 :     return false;
+     164             :   }
+     165             : 
+     166             :   // | ---------------- load plugin's parameters ---------------- |
+     167             : 
+     168         109 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/speed_tracker.yaml");
+     169         109 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/speed_tracker.yaml");
+     170             : 
+     171         218 :   const std::string yaml_prefix = "mrs_uav_trackers/speed_tracker/";
+     172             : 
+     173         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "command_timeout", _external_command_timeout_);
+     174             : 
+     175         109 :   private_handlers->param_loader->loadParam(yaml_prefix + "backup_tracker", _backup_tracker_);
+     176             : 
+     177         109 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     178           0 :     ROS_ERROR("[SpeedTracker]: could not load all parameters!");
+     179           0 :     return false;
+     180             :   }
+     181             : 
+     182             :   // | ------------------------ profiler ------------------------ |
+     183             : 
+     184         109 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "SpeedTracker", _profiler_enabled_);
+     185             : 
+     186             :   // | ----------------------- subscribers ---------------------- |
+     187             : 
+     188         109 :   mrs_lib::SubscribeHandlerOptions shopts;
+     189         109 :   shopts.nh              = nh_;
+     190         109 :   shopts.node_name       = "SpeedTracker";
+     191         109 :   shopts.threadsafe      = true;
+     192         109 :   shopts.autostart       = true;
+     193         109 :   shopts.transport_hints = ros::TransportHints().tcpNoDelay();
+     194             : 
+     195         109 :   sh_command_ = mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand>(shopts, "command", &SpeedTracker::callbackCommand, this);
+     196             : 
+     197             :   // | ----------------------- publishers ----------------------- |
+     198             : 
+     199         109 :   ph_rviz_marker_ = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "rviz_marker", 1);
+     200             : 
+     201             :   // | --------------------- service clients -------------------- |
+     202             : 
+     203         109 :   sch_switch_tracker_ = mrs_lib::ServiceClientHandler<mrs_msgs::String>(common_handlers->parent_nh, "switch_tracker");
+     204             : 
+     205             :   // | --------------------- finish the init -------------------- |
+     206             : 
+     207         109 :   is_initialized_ = true;
+     208             : 
+     209         109 :   ROS_INFO("[SpeedTracker]: initialized");
+     210             : 
+     211         109 :   return true;
+     212             : }
+     213             : 
+     214             : //}
+     215             : 
+     216             : /* //{ activate() */
+     217             : 
+     218           0 : std::tuple<bool, std::string> SpeedTracker::activate([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd) {
+     219             : 
+     220           0 :   std::stringstream ss;
+     221             : 
+     222           0 :   if (!got_uav_state_) {
+     223           0 :     ss << "odometry not set";
+     224           0 :     ROS_ERROR_STREAM("[SpeedTracker]: " << ss.str());
+     225           0 :     return std::tuple(false, ss.str());
+     226             :   }
+     227             : 
+     228           0 :   if (!getting_cmd_) {
+     229             : 
+     230           0 :     std::scoped_lock lock(mutex_command_);
+     231             : 
+     232           0 :     command_.header.stamp    = ros::Time(0);
+     233           0 :     command_.header.frame_id = last_tracker_cmd->header.frame_id;
+     234             : 
+     235           0 :     command_.use_velocity     = true;
+     236           0 :     command_.use_acceleration = false;
+     237           0 :     command_.use_force        = false;
+     238           0 :     command_.use_z            = false;
+     239           0 :     command_.use_heading_rate = false;
+     240             : 
+     241           0 :     command_.velocity.x = 0;
+     242           0 :     command_.velocity.y = 0;
+     243           0 :     command_.velocity.z = 0;
+     244             : 
+     245           0 :     if (last_tracker_cmd->use_heading) {
+     246           0 :       command_.heading     = last_tracker_cmd->heading;
+     247           0 :       command_.use_heading = true;
+     248             :     } else {
+     249           0 :       command_.use_heading = false;
+     250             :     }
+     251             :   }
+     252             : 
+     253           0 :   is_active_       = true;
+     254           0 :   first_iteration_ = true;
+     255             : 
+     256           0 :   ss << "activated";
+     257           0 :   ROS_INFO_STREAM("[SpeedTracker]: " << ss.str());
+     258             : 
+     259           0 :   return std::tuple(true, ss.str());
+     260             : }
+     261             : 
+     262             : //}
+     263             : 
+     264             : /* //{ deactivate() */
+     265             : 
+     266          20 : void SpeedTracker::deactivate(void) {
+     267             : 
+     268          20 :   is_active_   = false;
+     269          20 :   getting_cmd_ = false;
+     270             : 
+     271          20 :   ROS_INFO("[SpeedTracker]: deactivated");
+     272          20 : }
+     273             : 
+     274             : //}
+     275             : 
+     276             : /* //{ resetStatic() */
+     277             : 
+     278           0 : bool SpeedTracker::resetStatic(void) {
+     279             : 
+     280           0 :   return false;
+     281             : }
+     282             : 
+     283             : //}
+     284             : 
+     285             : /* //{ update() */
+     286             : 
+     287      142109 : std::optional<mrs_msgs::TrackerCommand> SpeedTracker::update(const mrs_msgs::UavState &                                          uav_state,
+     288             :                                                              [[maybe_unused]] const mrs_uav_managers::Controller::ControlOutput &last_control_output) {
+     289             : 
+     290      426327 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     291      426327 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("SpeedTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     292             : 
+     293             :   {
+     294      142109 :     std::scoped_lock lock(mutex_uav_state_);
+     295             : 
+     296      142109 :     uav_state_ = uav_state;
+     297             : 
+     298      142109 :     got_uav_state_ = true;
+     299             :   }
+     300             : 
+     301             :   double uav_heading;
+     302             : 
+     303             :   try {
+     304      142109 :     uav_heading = mrs_lib::AttitudeConverter(uav_state_.pose.orientation).getHeading();
+     305             :   }
+     306           0 :   catch (...) {
+     307           0 :     ROS_ERROR_THROTTLE(1.0, "[SpeedTracker]: could not calculate UAV heading");
+     308             : 
+     309           0 :     return {};
+     310             :   }
+     311             : 
+     312             :   // up to this part the update() method is evaluated even when the tracker is not active
+     313      142109 :   if (!is_active_) {
+     314      142109 :     return {};
+     315             :   }
+     316             : 
+     317           0 :   ros::Time external_command_time = sh_command_.lastMsgTime();
+     318             : 
+     319             :   // timeout the external command
+     320           0 :   if (getting_cmd_ && (ros::Time::now() - external_command_time).toSec() > _external_command_timeout_) {
+     321             : 
+     322           0 :     ROS_WARN("[SpeedTracker]: command timeouted");
+     323           0 :     getting_cmd_ = false;
+     324             : 
+     325           0 :     mrs_msgs::String srv;
+     326           0 :     srv.request.value = _backup_tracker_;
+     327             : 
+     328           0 :     switch_tracker_future_ = sch_switch_tracker_.callAsync(srv);
+     329             : 
+     330           0 :     switch_tracker_called_ = true;
+     331             :   }
+     332             : 
+     333           0 :   if (switch_tracker_called_ && switch_tracker_future_.wait_for(std::chrono::milliseconds(1)) == std::future_status::ready) {
+     334             : 
+     335           0 :     switch_tracker_called_ = false;
+     336             : 
+     337           0 :     if (!switch_tracker_future_.get().response.success) {
+     338           0 :       ROS_ERROR("[SpeedTracker]: failed to switch to backup tracker");
+     339           0 :       return {};
+     340             :     }
+     341             : 
+     342           0 :     switch_tracker_future_ = std::future<mrs_msgs::String>();
+     343             :   }
+     344             : 
+     345           0 :   auto command = mrs_lib::get_mutexed(mutex_command_, command_);
+     346             : 
+     347           0 :   mrs_msgs::TrackerCommand tracker_cmd;
+     348             : 
+     349           0 :   tracker_cmd.header.stamp    = ros::Time::now();
+     350           0 :   tracker_cmd.header.frame_id = uav_state.header.frame_id;
+     351             : 
+     352           0 :   tracker_cmd.position.x = uav_state.pose.position.x;
+     353           0 :   tracker_cmd.position.y = uav_state.pose.position.y;
+     354             : 
+     355           0 :   if (command.use_velocity) {
+     356           0 :     tracker_cmd.velocity.x              = command.velocity.x;
+     357           0 :     tracker_cmd.velocity.y              = command.velocity.y;
+     358           0 :     tracker_cmd.velocity.z              = command.velocity.z;
+     359           0 :     tracker_cmd.use_velocity_horizontal = true;
+     360           0 :     tracker_cmd.use_velocity_vertical   = true;
+     361             :   } else {
+     362           0 :     tracker_cmd.velocity.x              = uav_state.velocity.linear.x;
+     363           0 :     tracker_cmd.velocity.y              = uav_state.velocity.linear.y;
+     364           0 :     tracker_cmd.velocity.z              = uav_state.velocity.linear.z;
+     365           0 :     tracker_cmd.use_velocity_horizontal = false;
+     366           0 :     tracker_cmd.use_velocity_vertical   = false;
+     367             :   }
+     368             : 
+     369           0 :   if (command.use_z) {
+     370           0 :     tracker_cmd.position.z            = command.z;
+     371           0 :     tracker_cmd.use_position_vertical = true;
+     372             :   } else {
+     373           0 :     tracker_cmd.position.z            = uav_state.pose.position.z;
+     374           0 :     tracker_cmd.use_position_vertical = false;
+     375             :   }
+     376             : 
+     377           0 :   if (command.use_acceleration) {
+     378           0 :     tracker_cmd.acceleration.x   = command.acceleration.x;
+     379           0 :     tracker_cmd.acceleration.y   = command.acceleration.y;
+     380           0 :     tracker_cmd.acceleration.z   = command.acceleration.z;
+     381           0 :     tracker_cmd.use_acceleration = true;
+     382           0 :   } else if (command.use_force) {
+     383           0 :     tracker_cmd.acceleration.x   = command.force.x / last_control_output.diagnostics.total_mass;
+     384           0 :     tracker_cmd.acceleration.y   = command.force.y / last_control_output.diagnostics.total_mass;
+     385           0 :     tracker_cmd.acceleration.z   = command.force.z / last_control_output.diagnostics.total_mass;
+     386           0 :     tracker_cmd.use_acceleration = true;
+     387             :   } else {
+     388           0 :     tracker_cmd.acceleration.x   = 0;
+     389           0 :     tracker_cmd.acceleration.y   = 0;
+     390           0 :     tracker_cmd.acceleration.z   = 0;
+     391           0 :     tracker_cmd.use_acceleration = false;
+     392             :   }
+     393             : 
+     394           0 :   if (command.use_heading) {
+     395           0 :     tracker_cmd.heading     = command.heading;
+     396           0 :     tracker_cmd.use_heading = true;
+     397             :   } else {
+     398           0 :     tracker_cmd.heading     = uav_heading;
+     399           0 :     tracker_cmd.use_heading = false;
+     400             :   }
+     401             : 
+     402           0 :   if (command.use_heading_rate) {
+     403           0 :     tracker_cmd.heading_rate     = command.heading_rate;
+     404           0 :     tracker_cmd.use_heading_rate = true;
+     405             :   } else {
+     406           0 :     tracker_cmd.heading_rate     = uav_state.velocity.angular.z;
+     407           0 :     tracker_cmd.use_heading_rate = false;
+     408             :   }
+     409             : 
+     410           0 :   return {tracker_cmd};
+     411             : }
+     412             : 
+     413             : //}
+     414             : 
+     415             : /* //{ getStatus() */
+     416             : 
+     417           0 : const mrs_msgs::TrackerStatus SpeedTracker::getStatus() {
+     418             : 
+     419           0 :   mrs_msgs::TrackerStatus tracker_status;
+     420             : 
+     421           0 :   tracker_status.active            = is_active_;
+     422           0 :   tracker_status.callbacks_enabled = callbacks_enabled_;
+     423             : 
+     424           0 :   if (!tracker_status.active || first_iteration_)
+     425           0 :     tracker_status.state = mrs_msgs::TrackerStatus::STATE_IDLE;
+     426             :   else
+     427           0 :     tracker_status.state = mrs_msgs::TrackerStatus::STATE_REFERENCE;
+     428             : 
+     429           0 :   return tracker_status;
+     430             : }
+     431             : 
+     432             : //}
+     433             : 
+     434             : /* //{ enableCallbacks() */
+     435             : 
+     436         312 : const std_srvs::SetBoolResponse::ConstPtr SpeedTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     437             : 
+     438         624 :   std_srvs::SetBoolResponse res;
+     439         624 :   std::stringstream         ss;
+     440             : 
+     441         312 :   if (cmd->data != callbacks_enabled_) {
+     442             : 
+     443          20 :     callbacks_enabled_ = cmd->data;
+     444             : 
+     445          20 :     ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+     446          20 :     ROS_INFO_STREAM_THROTTLE(1.0, "[SpeedTracker]: " << ss.str());
+     447             : 
+     448             :   } else {
+     449             : 
+     450         292 :     ss << "callbacks were already " << (callbacks_enabled_ ? "enabled" : "disabled");
+     451         292 :     ROS_WARN_STREAM_THROTTLE(1.0, "[SpeedTracker]: " << ss.str());
+     452             :   }
+     453             : 
+     454         312 :   res.message = ss.str();
+     455         312 :   res.success = true;
+     456             : 
+     457         624 :   return std_srvs::SetBoolResponse::ConstPtr(new std_srvs::SetBoolResponse(res));
+     458             : }
+     459             : 
+     460             : //}
+     461             : 
+     462             : /* switchOdometrySource() //{ */
+     463             : 
+     464           0 : const std_srvs::TriggerResponse::ConstPtr SpeedTracker::switchOdometrySource([[maybe_unused]] const mrs_msgs::UavState &new_uav_state) {
+     465             : 
+     466           0 :   return std_srvs::TriggerResponse::Ptr();
+     467             : }
+     468             : 
+     469             : //}
+     470             : 
+     471             : /* //{ hover() */
+     472             : 
+     473           0 : const std_srvs::TriggerResponse::ConstPtr SpeedTracker::hover([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     474             : 
+     475           0 :   ROS_WARN("[SpeedTracker]: initiating hover");
+     476             : 
+     477           0 :   getting_cmd_ = false;
+     478             : 
+     479           0 :   mrs_msgs::String srv;
+     480           0 :   srv.request.value = _backup_tracker_;
+     481             : 
+     482           0 :   switch_tracker_future_ = sch_switch_tracker_.callAsync(srv);
+     483             : 
+     484           0 :   switch_tracker_called_ = true;
+     485             : 
+     486           0 :   std::stringstream ss;
+     487           0 :   ss << "initiating hover";
+     488             : 
+     489           0 :   std_srvs::TriggerResponse res;
+     490           0 :   res.success = true;
+     491           0 :   res.message = ss.str();
+     492             : 
+     493           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+     494             : }
+     495             : 
+     496             : //}
+     497             : 
+     498             : /* //{ startTrajectoryTracking() */
+     499             : 
+     500           0 : const std_srvs::TriggerResponse::ConstPtr SpeedTracker::startTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     501           0 :   return std_srvs::TriggerResponse::Ptr();
+     502             : }
+     503             : 
+     504             : //}
+     505             : 
+     506             : /* //{ stopTrajectoryTracking() */
+     507             : 
+     508           0 : const std_srvs::TriggerResponse::ConstPtr SpeedTracker::stopTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     509           0 :   return std_srvs::TriggerResponse::Ptr();
+     510             : }
+     511             : 
+     512             : //}
+     513             : 
+     514             : /* //{ resumeTrajectoryTracking() */
+     515             : 
+     516           0 : const std_srvs::TriggerResponse::ConstPtr SpeedTracker::resumeTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     517           0 :   return std_srvs::TriggerResponse::Ptr();
+     518             : }
+     519             : 
+     520             : //}
+     521             : 
+     522             : /* //{ gotoTrajectoryStart() */
+     523             : 
+     524           0 : const std_srvs::TriggerResponse::ConstPtr SpeedTracker::gotoTrajectoryStart([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     525           0 :   return std_srvs::TriggerResponse::Ptr();
+     526             : }
+     527             : 
+     528             : //}
+     529             : 
+     530             : /* //{ setConstraints() */
+     531             : 
+     532         387 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr SpeedTracker::setConstraints([
+     533             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     534             : 
+     535             :   {
+     536         387 :     std::scoped_lock lock(mutex_constraints_);
+     537             : 
+     538         387 :     constraints_ = cmd->constraints;
+     539             :   }
+     540             : 
+     541         774 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     542             : 
+     543         387 :   res.success = true;
+     544         387 :   res.message = "constraints updated";
+     545             : 
+     546         774 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     547             : }
+     548             : 
+     549             : //}
+     550             : 
+     551             : /* //{ setReference() */
+     552             : 
+     553           0 : const mrs_msgs::ReferenceSrvResponse::ConstPtr SpeedTracker::setReference([[maybe_unused]] const mrs_msgs::ReferenceSrvRequest::ConstPtr &cmd) {
+     554             : 
+     555           0 :   return mrs_msgs::ReferenceSrvResponse::Ptr();
+     556             : }
+     557             : 
+     558             : //}
+     559             : 
+     560             : /* //{ setVelocityReference() */
+     561             : 
+     562           0 : const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr SpeedTracker::setVelocityReference([
+     563             :     [maybe_unused]] const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr &cmd) {
+     564           0 :   return mrs_msgs::VelocityReferenceSrvResponse::Ptr();
+     565             : }
+     566             : 
+     567             : //}
+     568             : 
+     569             : /* //{ setTrajectoryReference() */
+     570             : 
+     571           6 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr SpeedTracker::setTrajectoryReference([
+     572             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     573           6 :   return mrs_msgs::TrajectoryReferenceSrvResponse::Ptr();
+     574             : }
+     575             : 
+     576             : //}
+     577             : 
+     578             : // | --------------------- custom methods --------------------- |
+     579             : 
+     580             : /* callbackCommand() //{ */
+     581             : 
+     582           0 : void SpeedTracker::callbackCommand(const mrs_msgs::SpeedTrackerCommand::ConstPtr msg) {
+     583             : 
+     584           0 :   if (!is_initialized_)
+     585           0 :     return;
+     586             : 
+     587           0 :   if (!is_active_)
+     588           0 :     return;
+     589             : 
+     590           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackCommand");
+     591           0 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("SpeedTracker::callbackCommand", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     592             : 
+     593           0 :   mrs_msgs::SpeedTrackerCommandConstPtr external_command = msg;
+     594             : 
+     595             :   double dt;
+     596             : 
+     597           0 :   if (first_iteration_) {
+     598             : 
+     599           0 :     last_command_time_ = ros::Time::now();
+     600           0 :     first_iteration_   = false;
+     601             : 
+     602             :     {
+     603           0 :       std::scoped_lock lock(mutex_command_);
+     604             : 
+     605           0 :       command_ = *external_command;
+     606             :     }
+     607             : 
+     608           0 :     return;
+     609             : 
+     610             :   } else {
+     611           0 :     dt                 = (ros::Time::now() - last_command_time_).toSec();
+     612           0 :     last_command_time_ = ros::Time::now();
+     613             : 
+     614           0 :     if (dt <= 1e-4) {
+     615           0 :       ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: the command dt is %.5f, returning", dt);
+     616           0 :       return;
+     617             :     }
+     618             :   }
+     619             : 
+     620           0 :   getting_cmd_ = true;
+     621             : 
+     622           0 :   mrs_msgs::SpeedTrackerCommand transformed_command = *external_command;
+     623             : 
+     624           0 :   auto old_command = mrs_lib::get_mutexed(mutex_command_, command_);
+     625           0 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     626           0 :   auto uav_state   = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     627             : 
+     628             :   double uav_heading;
+     629             : 
+     630             :   try {
+     631           0 :     uav_heading = mrs_lib::AttitudeConverter(uav_state_.pose.orientation).getHeading();
+     632             :   }
+     633           0 :   catch (...) {
+     634           0 :     ROS_ERROR_THROTTLE(1.0, "[SpeedTracker]: could not calculate UAV heading");
+     635           0 :     return;
+     636             :   }
+     637             : 
+     638             :   // transform the command
+     639             : 
+     640             :   // transform velocity
+     641             : 
+     642           0 :   if (transformed_command.use_velocity) {
+     643             : 
+     644           0 :     geometry_msgs::Vector3Stamped vector3;
+     645           0 :     vector3.header = transformed_command.header;
+     646             : 
+     647           0 :     vector3.vector.x = transformed_command.velocity.x;
+     648           0 :     vector3.vector.y = transformed_command.velocity.y;
+     649           0 :     vector3.vector.z = transformed_command.velocity.z;
+     650             : 
+     651           0 :     auto ret = common_handlers_->transformer->transformSingle(vector3, "");
+     652             : 
+     653           0 :     if (ret) {
+     654           0 :       transformed_command.velocity.x = ret.value().vector.x;
+     655           0 :       transformed_command.velocity.y = ret.value().vector.y;
+     656           0 :       transformed_command.velocity.z = ret.value().vector.z;
+     657             :     } else {
+     658           0 :       return;
+     659             :     }
+     660             : 
+     661             :     /* horizontal speed limit //{ */
+     662             : 
+     663             :     {
+     664           0 :       double des_horizontal_speed = sqrt(pow(transformed_command.velocity.x, 2) + pow(transformed_command.velocity.y, 2));
+     665             : 
+     666           0 :       if (des_horizontal_speed > constraints.horizontal_speed) {
+     667             : 
+     668           0 :         double des_speed_heading = atan2(transformed_command.velocity.y, transformed_command.velocity.x);
+     669             : 
+     670           0 :         transformed_command.velocity.x = cos(des_speed_heading) * constraints.horizontal_speed;
+     671           0 :         transformed_command.velocity.y = sin(des_speed_heading) * constraints.horizontal_speed;
+     672             :       }
+     673             :     }
+     674             : 
+     675             :     //}
+     676             : 
+     677             :     /* horizontal speed change rate limit //{ */
+     678             : 
+     679             :     {
+     680             :       Eigen::Vector2d hor_speed_derivative =
+     681           0 :           Eigen::Vector2d(transformed_command.velocity.x - old_command.velocity.x, transformed_command.velocity.y - old_command.velocity.y) / dt;
+     682             : 
+     683             :       // exceeding the maximum acceleration
+     684           0 :       if (hor_speed_derivative.norm() > constraints.horizontal_acceleration) {
+     685             : 
+     686           0 :         ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: limitting speed change rate");
+     687           0 :         double direction = atan2(hor_speed_derivative[1], hor_speed_derivative[0]);
+     688             : 
+     689           0 :         transformed_command.velocity.x = old_command.velocity.x + cos(direction) * constraints.horizontal_acceleration * dt;
+     690           0 :         transformed_command.velocity.y = old_command.velocity.y + sin(direction) * constraints.horizontal_acceleration * dt;
+     691             :       }
+     692             :     }
+     693             : 
+     694             :     //}
+     695             : 
+     696             :     /* vertical speed limit //{ */
+     697             : 
+     698             :     {
+     699             :       // if ascending
+     700           0 :       if (transformed_command.velocity.z > constraints.vertical_ascending_speed) {
+     701             : 
+     702           0 :         transformed_command.velocity.z = constraints.vertical_ascending_speed;
+     703             :       }
+     704             : 
+     705             :       // if descending
+     706           0 :       if (transformed_command.velocity.z < -constraints.vertical_descending_speed) {
+     707             : 
+     708           0 :         transformed_command.velocity.z = -constraints.vertical_descending_speed;
+     709             :       }
+     710             :     }
+     711             : 
+     712             :     //}
+     713             : 
+     714             :     /* vertical speed change rate //{ */
+     715             : 
+     716             :     {
+     717             : 
+     718           0 :       double vert_speed_derivative = (transformed_command.velocity.z - old_command.velocity.z) / dt;
+     719             : 
+     720           0 :       if (vert_speed_derivative > constraints.vertical_ascending_acceleration) {
+     721             : 
+     722           0 :         ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: limitting vertical ascending speed change rate");
+     723           0 :         transformed_command.velocity.z = old_command.velocity.z + constraints.vertical_ascending_acceleration * dt;
+     724             : 
+     725           0 :       } else if (vert_speed_derivative < -constraints.vertical_descending_acceleration) {
+     726             : 
+     727           0 :         ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: limitting vertical descending speed change rate");
+     728           0 :         transformed_command.velocity.z = old_command.velocity.z - constraints.vertical_descending_acceleration * dt;
+     729             :       }
+     730             :     }
+     731             : 
+     732             :     //}
+     733             :   }
+     734             : 
+     735             :   /* transform and constrain heading //{ */
+     736             : 
+     737           0 :   if (transformed_command.use_heading) {
+     738             : 
+     739           0 :     mrs_msgs::ReferenceStamped temp_ref;
+     740           0 :     temp_ref.header = transformed_command.header;
+     741             : 
+     742           0 :     temp_ref.reference.heading = transformed_command.heading;
+     743             : 
+     744           0 :     auto ret = common_handlers_->transformer->transformSingle(temp_ref, "");
+     745             : 
+     746           0 :     if (ret) {
+     747             : 
+     748             :       // calculate the produced heading rate
+     749           0 :       double des_hdg_rate = sradians::diff(ret.value().reference.heading, old_command.heading) / dt;
+     750             : 
+     751             :       // saturate the change in the desired heading
+     752           0 :       if (des_hdg_rate > constraints.heading_speed) {
+     753             : 
+     754           0 :         ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: limitting change of the desired heading using constraints");
+     755           0 :         transformed_command.heading = old_command.heading + constraints.heading_speed * dt;
+     756             : 
+     757           0 :       } else if (des_hdg_rate < -constraints.heading_speed) {
+     758             : 
+     759           0 :         ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: limitting change of the desired heading using constraints");
+     760           0 :         transformed_command.heading = old_command.heading - constraints.heading_speed * dt;
+     761             : 
+     762             :       } else {
+     763             : 
+     764           0 :         transformed_command.heading = ret.value().reference.heading;
+     765             :       }
+     766             : 
+     767             :     } else {
+     768           0 :       return;
+     769             :     }
+     770             :   } else {
+     771           0 :     transformed_command.use_heading = false;
+     772           0 :     transformed_command.heading     = uav_heading;
+     773             :   }
+     774             : 
+     775             :   //}
+     776             : 
+     777           0 :   if (transformed_command.use_heading_rate) {
+     778             : 
+     779           0 :     if (transformed_command.heading_rate > constraints.heading_speed) {
+     780           0 :       transformed_command.heading_rate = constraints.heading_speed;
+     781           0 :     } else if (transformed_command.heading_rate < -constraints.heading_speed) {
+     782           0 :       transformed_command.heading_rate = -constraints.heading_speed;
+     783             :     }
+     784             :   }
+     785             : 
+     786           0 :   if (transformed_command.use_acceleration) {
+     787             : 
+     788           0 :     geometry_msgs::Vector3Stamped vector3;
+     789           0 :     vector3.header = transformed_command.header;
+     790             : 
+     791           0 :     vector3.vector.x = transformed_command.acceleration.x;
+     792           0 :     vector3.vector.y = transformed_command.acceleration.y;
+     793           0 :     vector3.vector.z = transformed_command.acceleration.z;
+     794             : 
+     795           0 :     auto ret = common_handlers_->transformer->transformSingle(vector3, "");
+     796             : 
+     797           0 :     if (ret) {
+     798           0 :       transformed_command.acceleration.x = ret.value().vector.x;
+     799           0 :       transformed_command.acceleration.y = ret.value().vector.y;
+     800           0 :       transformed_command.acceleration.z = ret.value().vector.z;
+     801             :     } else {
+     802           0 :       return;
+     803             :     }
+     804             : 
+     805             :     /* horizontal acceleration limit //{ */
+     806             : 
+     807             :     {
+     808           0 :       double des_horizontal_acceleration = sqrt(pow(transformed_command.acceleration.x, 2) + pow(transformed_command.acceleration.y, 2));
+     809             : 
+     810           0 :       if (des_horizontal_acceleration > constraints.horizontal_acceleration) {
+     811             : 
+     812           0 :         double des_acc_heading = atan2(transformed_command.acceleration.y, transformed_command.acceleration.x);
+     813             : 
+     814           0 :         transformed_command.acceleration.x = cos(des_acc_heading) * constraints.horizontal_acceleration;
+     815           0 :         transformed_command.acceleration.y = sin(des_acc_heading) * constraints.horizontal_acceleration;
+     816             :       }
+     817             :     }
+     818             : 
+     819             :     //}
+     820             : 
+     821             :     /* horizontal acceleration change rate limit //{ */
+     822             : 
+     823             :     {
+     824             :       Eigen::Vector2d hor_acc_derivative =
+     825           0 :           Eigen::Vector2d(transformed_command.acceleration.x - old_command.acceleration.x, transformed_command.acceleration.y - old_command.acceleration.y) /
+     826           0 :           (dt);
+     827             : 
+     828             :       // exceeding the maximum acceleration
+     829           0 :       if (hor_acc_derivative.norm() > constraints.horizontal_jerk) {
+     830             : 
+     831           0 :         ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: limitting acceleration change rate");
+     832           0 :         double direction = atan2(hor_acc_derivative[1], hor_acc_derivative[0]);
+     833             : 
+     834           0 :         transformed_command.acceleration.x = old_command.acceleration.x + cos(direction) * constraints.horizontal_jerk * dt;
+     835           0 :         transformed_command.acceleration.y = old_command.acceleration.y + sin(direction) * constraints.horizontal_jerk * dt;
+     836             :       }
+     837             :     }
+     838             : 
+     839             :     //}
+     840             : 
+     841             :     /* vertical acceleration limit //{ */
+     842             : 
+     843             :     {
+     844             :       // if ascending
+     845           0 :       if (transformed_command.acceleration.z > constraints.vertical_ascending_acceleration) {
+     846             : 
+     847           0 :         transformed_command.acceleration.z = constraints.vertical_ascending_acceleration;
+     848             :       }
+     849             : 
+     850             :       // if descending
+     851           0 :       if (transformed_command.acceleration.z < -constraints.vertical_descending_acceleration) {
+     852             : 
+     853           0 :         transformed_command.acceleration.z = -constraints.vertical_descending_acceleration;
+     854             :       }
+     855             :     }
+     856             : 
+     857             :     //}
+     858             : 
+     859             :     /* vertical acceleration change rate //{ */
+     860             : 
+     861             :     {
+     862             : 
+     863           0 :       double vert_acc_derivative = (transformed_command.acceleration.z - old_command.acceleration.z) / dt;
+     864             : 
+     865           0 :       if (vert_acc_derivative > constraints.vertical_ascending_jerk) {
+     866             : 
+     867           0 :         ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: limitting vertical ascending acceleration change rate");
+     868           0 :         transformed_command.acceleration.z = old_command.acceleration.z + constraints.vertical_ascending_jerk * dt;
+     869             : 
+     870           0 :       } else if (vert_acc_derivative < -constraints.vertical_descending_jerk) {
+     871             : 
+     872           0 :         ROS_WARN_THROTTLE(1.0, "[SpeedTracker]: limitting vertical descending acceleration change rate");
+     873           0 :         transformed_command.acceleration.z = old_command.acceleration.z - constraints.vertical_descending_jerk * dt;
+     874             :       }
+     875             :     }
+     876             : 
+     877             :     //}
+     878             :   }
+     879             : 
+     880             :   // transform force
+     881             : 
+     882           0 :   if (transformed_command.use_force) {
+     883             : 
+     884           0 :     geometry_msgs::Vector3Stamped vector3;
+     885           0 :     vector3.header = transformed_command.header;
+     886             : 
+     887           0 :     vector3.vector.x = transformed_command.force.x;
+     888           0 :     vector3.vector.y = transformed_command.force.y;
+     889           0 :     vector3.vector.z = transformed_command.force.z;
+     890             : 
+     891           0 :     auto ret = common_handlers_->transformer->transformSingle(vector3, "");
+     892             : 
+     893           0 :     if (ret) {
+     894           0 :       transformed_command.force.x = vector3.vector.x;
+     895           0 :       transformed_command.force.y = vector3.vector.y;
+     896           0 :       transformed_command.force.z = vector3.vector.z;
+     897             :     } else {
+     898           0 :       return;
+     899             :     }
+     900             :   }
+     901             : 
+     902             :   // check the feasibility of the z
+     903             :   {
+     904           0 :     double z_derivative = (transformed_command.z - old_command.z) / dt;
+     905             : 
+     906           0 :     if (z_derivative > constraints.vertical_ascending_speed) {
+     907             : 
+     908           0 :       transformed_command.z = old_command.z + constraints.vertical_ascending_speed * dt;
+     909             : 
+     910           0 :     } else if (z_derivative < -constraints.vertical_ascending_speed) {
+     911             : 
+     912           0 :       transformed_command.z = old_command.z - constraints.vertical_descending_speed * dt;
+     913             :     }
+     914             : 
+     915             :     // saturate the desired z using the safety area
+     916           0 :     if (common_handlers_->safety_area.use_safety_area) {
+     917             : 
+     918           0 :       if (transformed_command.z > common_handlers_->safety_area.getMaxZ("")) {
+     919             : 
+     920           0 :         transformed_command.z = common_handlers_->safety_area.getMaxZ("");
+     921             : 
+     922           0 :       } else if (transformed_command.z < common_handlers_->safety_area.getMinZ("")) {
+     923             : 
+     924           0 :         transformed_command.z = common_handlers_->safety_area.getMinZ("");
+     925             :       }
+     926             :     }
+     927             :   }
+     928             : 
+     929             :   // if not active, nullify the desired speeds and accelerations
+     930             :   // this will produce a rumpum (using the constraints) after the activation
+     931           0 :   if (!is_active_) {
+     932             : 
+     933           0 :     auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     934             : 
+     935           0 :     transformed_command.velocity.x = 0;
+     936           0 :     transformed_command.velocity.y = 0;
+     937           0 :     transformed_command.velocity.z = 0;
+     938             : 
+     939           0 :     transformed_command.acceleration.x = 0;
+     940           0 :     transformed_command.acceleration.y = 0;
+     941           0 :     transformed_command.acceleration.z = 0;
+     942             : 
+     943             :     try {
+     944           0 :       transformed_command.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     945             :     }
+     946           0 :     catch (...) {
+     947           0 :       return;
+     948             :     }
+     949             : 
+     950           0 :     transformed_command.z = uav_state_.pose.position.z;
+     951             :   }
+     952             : 
+     953             :   {
+     954           0 :     std::scoped_lock lock(mutex_command_);
+     955             : 
+     956           0 :     command_ = transformed_command;
+     957             :   }
+     958             : 
+     959           0 :   if (!is_active_) {
+     960           0 :     ROS_INFO_ONCE("[SpeedTracker]: getting command");
+     961             :   } else {
+     962           0 :     ROS_INFO_THROTTLE(5.0, "[SpeedTracker]: getting command");
+     963             :   }
+     964             : 
+     965             :   // --------------------------------------------------------------
+     966             :   // |                     publish rviz markers                   |
+     967             :   // --------------------------------------------------------------
+     968             : 
+     969           0 :   visualization_msgs::MarkerArray msg_out;
+     970             : 
+     971           0 :   double id = 0;
+     972             : 
+     973           0 :   geometry_msgs::Point point;
+     974             : 
+     975             :   /* desired speed //{ */
+     976             : 
+     977           0 :   if (transformed_command.use_velocity) {
+     978             : 
+     979           0 :     std::scoped_lock lock(mutex_uav_state_);
+     980             : 
+     981           0 :     visualization_msgs::Marker marker;
+     982             : 
+     983           0 :     marker.header.frame_id = uav_state_.header.frame_id;
+     984           0 :     marker.header.stamp    = ros::Time::now();
+     985           0 :     marker.ns              = "speed_tracker";
+     986           0 :     marker.id              = id++;
+     987           0 :     marker.type            = visualization_msgs::Marker::ARROW;
+     988           0 :     marker.action          = visualization_msgs::Marker::ADD;
+     989             : 
+     990             :     /* position //{ */
+     991             : 
+     992           0 :     marker.pose.position.x = 0.0;
+     993           0 :     marker.pose.position.y = 0.0;
+     994           0 :     marker.pose.position.z = 0.0;
+     995             : 
+     996             :     //}
+     997             : 
+     998             :     /* orientation //{ */
+     999             : 
+    1000           0 :     marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    1001             : 
+    1002             :     //}
+    1003             : 
+    1004             :     /* origin //{ */
+    1005           0 :     point.x = uav_state_.pose.position.x;
+    1006           0 :     point.y = uav_state_.pose.position.y;
+    1007           0 :     point.z = uav_state_.pose.position.z;
+    1008             : 
+    1009           0 :     marker.points.push_back(point);
+    1010             : 
+    1011             :     //}
+    1012             : 
+    1013             :     /* tip //{ */
+    1014             : 
+    1015           0 :     point.x = uav_state_.pose.position.x + transformed_command.velocity.x;
+    1016           0 :     point.y = uav_state_.pose.position.y + transformed_command.velocity.y;
+    1017           0 :     point.z = uav_state_.pose.position.z + transformed_command.velocity.z;
+    1018             : 
+    1019           0 :     marker.points.push_back(point);
+    1020             : 
+    1021             :     //}
+    1022             : 
+    1023           0 :     marker.scale.x = 0.05;
+    1024           0 :     marker.scale.y = 0.05;
+    1025           0 :     marker.scale.z = 0.05;
+    1026             : 
+    1027           0 :     marker.color.a = 0.5;
+    1028           0 :     marker.color.r = 0.0;
+    1029           0 :     marker.color.g = 1.0;
+    1030           0 :     marker.color.b = 0.0;
+    1031             : 
+    1032           0 :     marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+    1033             : 
+    1034           0 :     msg_out.markers.push_back(marker);
+    1035             :   }
+    1036             : 
+    1037             :   //}
+    1038             : 
+    1039             :   /* desired acceleration //{ */
+    1040           0 :   if (transformed_command.use_acceleration) {
+    1041             : 
+    1042           0 :     std::scoped_lock lock(mutex_uav_state_);
+    1043             : 
+    1044           0 :     visualization_msgs::Marker marker;
+    1045             : 
+    1046           0 :     marker.header.frame_id = uav_state_.header.frame_id;
+    1047           0 :     marker.header.stamp    = ros::Time::now();
+    1048           0 :     marker.ns              = "speed_tracker";
+    1049           0 :     marker.id              = id++;
+    1050           0 :     marker.type            = visualization_msgs::Marker::ARROW;
+    1051           0 :     marker.action          = visualization_msgs::Marker::ADD;
+    1052             : 
+    1053             :     /* position //{ */
+    1054             : 
+    1055           0 :     marker.pose.position.x = 0.0;
+    1056           0 :     marker.pose.position.y = 0.0;
+    1057           0 :     marker.pose.position.z = 0.0;
+    1058             : 
+    1059             :     //}
+    1060             : 
+    1061             :     /* orientation //{ */
+    1062             : 
+    1063           0 :     marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    1064             : 
+    1065             :     //}
+    1066             : 
+    1067             :     /* origin //{ */
+    1068           0 :     point.x = uav_state_.pose.position.x;
+    1069           0 :     point.y = uav_state_.pose.position.y;
+    1070           0 :     point.z = uav_state_.pose.position.z;
+    1071             : 
+    1072           0 :     marker.points.push_back(point);
+    1073             : 
+    1074             :     //}
+    1075             : 
+    1076             :     /* tip //{ */
+    1077             : 
+    1078           0 :     point.x = uav_state_.pose.position.x + transformed_command.acceleration.x;
+    1079           0 :     point.y = uav_state_.pose.position.y + transformed_command.acceleration.y;
+    1080           0 :     point.z = uav_state_.pose.position.z + transformed_command.acceleration.z;
+    1081             : 
+    1082           0 :     marker.points.push_back(point);
+    1083             : 
+    1084             :     //}
+    1085             : 
+    1086           0 :     marker.scale.x = 0.05;
+    1087           0 :     marker.scale.y = 0.05;
+    1088           0 :     marker.scale.z = 0.05;
+    1089             : 
+    1090           0 :     marker.color.a = 0.5;
+    1091           0 :     marker.color.r = 1.0;
+    1092           0 :     marker.color.g = 0.0;
+    1093           0 :     marker.color.b = 0.0;
+    1094             : 
+    1095           0 :     marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+    1096             : 
+    1097           0 :     msg_out.markers.push_back(marker);
+    1098             :   }
+    1099             : 
+    1100             :   //}
+    1101             : 
+    1102             :   /* desired force //{ */
+    1103           0 :   if (transformed_command.use_force) {
+    1104             : 
+    1105           0 :     std::scoped_lock lock(mutex_uav_state_);
+    1106             : 
+    1107           0 :     visualization_msgs::Marker marker;
+    1108             : 
+    1109           0 :     marker.header.frame_id = uav_state_.header.frame_id;
+    1110           0 :     marker.header.stamp    = ros::Time::now();
+    1111           0 :     marker.ns              = "speed_tracker";
+    1112           0 :     marker.id              = id++;
+    1113           0 :     marker.type            = visualization_msgs::Marker::ARROW;
+    1114           0 :     marker.action          = visualization_msgs::Marker::ADD;
+    1115             : 
+    1116             :     /* position //{ */
+    1117             : 
+    1118           0 :     marker.pose.position.x = 0.0;
+    1119           0 :     marker.pose.position.y = 0.0;
+    1120           0 :     marker.pose.position.z = 0.0;
+    1121             : 
+    1122             :     //}
+    1123             : 
+    1124             :     /* orientation //{ */
+    1125             : 
+    1126           0 :     marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    1127             : 
+    1128             :     //}
+    1129             : 
+    1130             :     /* origin //{ */
+    1131           0 :     point.x = uav_state_.pose.position.x;
+    1132           0 :     point.y = uav_state_.pose.position.y;
+    1133           0 :     point.z = uav_state_.pose.position.z;
+    1134             : 
+    1135           0 :     marker.points.push_back(point);
+    1136             : 
+    1137             :     //}
+    1138             : 
+    1139             :     /* tip //{ */
+    1140             : 
+    1141           0 :     point.x = uav_state_.pose.position.x + transformed_command.force.x;
+    1142           0 :     point.y = uav_state_.pose.position.y + transformed_command.force.y;
+    1143           0 :     point.z = uav_state_.pose.position.z + transformed_command.force.z;
+    1144             : 
+    1145           0 :     marker.points.push_back(point);
+    1146             : 
+    1147             :     //}
+    1148             : 
+    1149           0 :     marker.scale.x = 0.05;
+    1150           0 :     marker.scale.y = 0.05;
+    1151           0 :     marker.scale.z = 0.05;
+    1152             : 
+    1153           0 :     marker.color.a = 0.5;
+    1154           0 :     marker.color.r = 0.0;
+    1155           0 :     marker.color.g = 0.0;
+    1156           0 :     marker.color.b = 1.0;
+    1157             : 
+    1158           0 :     marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+    1159             : 
+    1160           0 :     msg_out.markers.push_back(marker);
+    1161             :   }
+    1162             : 
+    1163             :   //}
+    1164             : 
+    1165           0 :   ph_rviz_marker_.publish(msg_out);
+    1166             : }
+    1167             : 
+    1168             : //}
+    1169             : 
+    1170             : }  // namespace speed_tracker
+    1171             : 
+    1172             : }  // namespace mrs_uav_trackers
+    1173             : 
+    1174             : #include <pluginlib/class_list_macros.h>
+    1175         109 : 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..ac79bb7468 --- /dev/null +++ b/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.overview.html @@ -0,0 +1,314 @@ + + + + + + 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 + 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..15a47cc08e8ce6407096d6160bf84282f233268c GIT binary patch literal 3278 zcmV;<3^DVGP)Rw1qPr%(kE%X#cI;QCNq%wx{*g<=s&V}*&TM@Hs^DsAHg@Ph6U38x`p zbTT|n5eB@SQj8idk{$e51v(By!sHYk1t1LxJi(xSioiJnUga^NDCw-U9;4?Rg`dB& zfFl=pAQ`~P^2isOiw+eCbXuVpjA^i( z6TjFQ$#s_SQv%u_MpqXB_jFj(9~O^5>d~_bOixZRZ&xbZo6#Y$-8h3Vlc{)`VKZJ3 z#*;W>hAQy&fCrpS6e8v;p~#w&iYSt0Y-*#3#XK;w4qQZW^sRip9t^iA%tUja=+3=> z1E&m#K$Aa&_HH)u)JY}Uylmk`VDiQ_Nrpt2afS})qA*DTe!PjIe_vzVBw1^wj?u!} zMlX};DRxLdj6j(1#gB{)6jf)Buk!Qr_=(Che20G-{>9~oD*UP;jek%7Q&uA2(*vIf zkXbCnM@8AjqFkF;9k6bE#)!aD6dhGz1P445kbGAXhPjC<^|1?V#kod)PJ zG4=R)Ys;i`f-y-b;;cPV;IizntjGU#-(LSrS!^-D*PpkgT!y8FaKO)K!eS8s1#N9S zj{W+~XgQ#OB2@tOhlV{y?ouW-)m2X30`O+`m=3{L>=7{qU?@U`Cp;!ISCu~V_(4Yz zD~$Q&H6oH@SfFTt{%0EaG(a}RI`AhQwPPlveTHhoP_8xMwWN!ay2fQIj~dgI&@yE% zFYq|U3lP`zSPIb^J+71cGQ~m<2tY=RX@Km~W!=@9zZOL>z`V+~=Vd$p54^$S7-zzZ z#SuqBarJCgDDLZKt1gX*>F9dAv6)IX1Q=@15gw>Mx(2C>LQC|xM1sVD#6^mSUe=W& znJ9c+_oQqA*i`jGws%tXw&60zz~grY(EQG#SgV>>AjKECUk37dOxqW=;DcgNcx)3J z1vJJ(SH0D`3qg_Sz6ZrRxdW)bt6_0AR*=~6$WV=wnkS6&qeXI2)q#Z#drvleI8#7L zIUv*F3OsJV@w#VP5szD>A=f!gD26~THe&!~RWqVJc5KG3x?@|b?Tg!aepKGMs$J(! zol9}=H8Q=5=Uxxioa^D=2f7^sH?pldFmR=dn z>DYKZd*5<+bDwZGTM1r>kjG6I5>R`5FwzhD_P9hXV&@;BK!tPl=ytnX)sV*NX8`L) z)NX>+w&|Xc3J*{{un^P>rH0ey3dLMl?1e_D^I0=o1!$#60y`!&V#`R&1!3q}9G_yY zW!I(&#ptg`<=~5N91E%gt?;7tgxb2a^{Ge1WaBwqLt4>ldna1Ek)lcCpk$Lt zF6BmBt1cuyBtU-EfJ|Y0kFCc(+>q*f1B2ExX0bdjnh6}3d7&DQ;c0JRkUH}ml++0D zok{6TYAP{=37s>3sR_|8OH`eE>h5D9-49b9<23270?BB>v<&FTa$%BjqCc=A4B5g( zVOR?g9e`x$)7x+hdWzN+KnJV{gZF(WjJ#z+)fHfAdFF#0K)g;;0P0(Kx-(KD)~96+nhZ*2;R`yE8Zu=RQTGMEi|Oo9nVS0DApD#S|X|P zLIvETSzM@4$!Nxw*-nuLiYZoirn*pYqZHFzwWV4C7N)^DP;H%SM_dZ>>`pFa){7ze zJ{;2{rk)$>QDHFr>QP6LUp?wX4F7vQDs7;{X4BX-Ak2`Sg;$=E1Me4iu;mkX`LsD~ z)}w%(rhA#DvwB+2?$zdn9_V#R03}ryj>dXBxyuA{RP9@+lfvB5%XFfUi2QE2b*1&S^M zmtxvZpdN1Qx-d`gN~B(5iLal5WH+IDpJA`~ysCCD1<8}{wJkEEVDflwE?(=$Dw55` z;WVV#_P+W^!vgDyeE%`l+S%0RWGalhfD05;Trook@oBW&lpc;~S3`Eilk#h~G_FGTn*v2UWldhmqb=JH30|h*;#Lga9 z)nTvFvnlnRd)e8PSg6iG(FF{uBbT&yxqF{A(gmU*E~pOosH^C(WV|Ccv9}ch*%Tut zF%v!>i78U%6weAKW?K8rqs6S4~kYC)O?Ya+?SCmJ^C$Y#mT zGS4g`7|fr#=^z#zU20Z`$#pTaAlFSHNRM? zE2@5*)PLE+?Ue#oDQdI0BvDG&E#En+_sLX9bd}8q_-evQyQzzlb$UUE0f&Kq{9Rh*^^uz#l$S^ z!Wq`q!0k~UYTH|4+{s-e<5ulbn_Pl_OiuYXAM;!OVBt>J)>8zj0NngTiBCK~Mtt8~JeRN(VilfX9!?hvWI{F2Gym!)@4n zeM3-VKAbSP_L=6cmMtVM7I9B6W;Q65SlvJ}QV3=Qkb|`X$cQ zHfl3J1Lo%Q>+KhS0vo?u3W_&(I@N;lviZQ~zGy7ea-dxJkDQu$4!G zW)~onNA?MkrM9GL3*SF3%o8H-Oq>_|M0}#V!wRG*0x-4^xWm=Mt!pDRnoM*!0VSS{I?jyv?6oBES(QYq4_LDNN3o>Nz9V;wO@} zPz()N{NxKJMfZ6~YA5Ks9@B!9btrnW6tIP2Vb%_T8#q}>QM8)#Ky-OO0akd7Zm5Q> zl<~Nsz?SV#U<#mOWceop@ot}20`jHqb+fBpca2V`CPB{@KK1GSQZtOl1XAVArlGBs zflT$}yTeEzlVZ2qaTIT}#q@}sQ|$u$LoE|!hFQWW0+~`nij~_<-U#HC$S4Y_Y3YZ9 zW@d^3Z?Nmr$B(tqz2|PlZ&>&Z3!RyL!$K8$->^^%$W+fAf{!#lg&3Q{Dy_j%)Viv3cYVws0C!I=MKTgTf#!O=}RB@hg#+v7JkFR z49G7X&WJPn3{#f3et^Jaj|?^LH>gt7*vUTo + + + + + + 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:755111867.5 %
Date:2024-12-17 22:35:29Functions: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 +
67.5%67.5%
+ 		67.5 %755 / 1118100.0 %22 / 22
<unnamed>67.5 %755 / 1118100.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..4528bbd26d --- /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:755111867.5 %
Date:2024-12-17 22:35:29Functions: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 +
67.5%67.5%
+ 		67.5 %755 / 1118100.0 %22 / 22
<unnamed>67.5 %755 / 1118100.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..9f6fde7ae3 --- /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:755111867.5 %
Date:2024-12-17 22:35:29Functions: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 +
67.5%67.5%
+ 		67.5 %755 / 1118100.0 %22 / 22
<unnamed>67.5 %755 / 1118100.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..43f526420f --- /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:755111867.5 %
Date:2024-12-17 22:35:29Functions: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 +
67.5%67.5%
+ 		67.5 %755 / 1118100.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..559b5bc64f --- /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:755111867.5 %
Date:2024-12-17 22:35:29Functions: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 +
67.5%67.5%
+ 		67.5 %755 / 1118100.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..96a2f6727f --- /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:755111867.5 %
Date:2024-12-17 22:35:29Functions: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 +
67.5%67.5%
+ 		67.5 %755 / 1118100.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..42d95a32b8 --- /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:755111867.5 %
Date:2024-12-17 22:35:29Functions: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()109
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackDrs(mrs_uav_trajectory_generation::drsConfig&, unsigned int)109
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::onInit()109
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::interpolatePoint(Waypoint_t const&, Waypoint_t const&, double const&)1014
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&)4924
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::overtime()9635
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)176092
+
+
+ + + +
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..c153fb816e --- /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:755111867.5 %
Date:2024-12-17 22:35:29Functions:2222100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()109
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackDrs(mrs_uav_trajectory_generation::drsConfig&, unsigned int)109
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&)4924
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)176092
mrs_uav_trajectory_generation::MrsTrajectoryGeneration::interpolatePoint(Waypoint_t const&, Waypoint_t const&, double const&)1014
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()109
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()9635
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..decb2a9c7c --- /dev/null +++ b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.html @@ -0,0 +1,2514 @@ + + + + + + + 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:755111867.5 %
Date:2024-12-17 22:35:29Functions: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             :   double _takeoff_height_;
+     106             : 
+     107             :   std::string _uav_name_;
+     108             : 
+     109             :   bool   _trajectory_max_segment_deviation_enabled_;
+     110             :   double trajectory_max_segment_deviation_;
+     111             :   int    _trajectory_max_segment_deviation_max_iterations_;
+     112             : 
+     113             :   bool   _path_straightener_enabled_;
+     114             :   double _path_straightener_max_deviation_;
+     115             :   double _path_straightener_max_hdg_deviation_;
+     116             : 
+     117             :   bool _override_heading_atan2_;
+     118             : 
+     119             :   // | -------- variable parameters (come with the path) -------- |
+     120             : 
+     121             :   std::string frame_id_;
+     122             :   bool        fly_now_                              = false;
+     123             :   bool        use_heading_                          = false;
+     124             :   bool        stop_at_waypoints_                    = false;
+     125             :   bool        override_constraints_                 = false;
+     126             :   bool        loop_                                 = false;
+     127             :   double      override_max_velocity_horizontal_     = 0.0;
+     128             :   double      override_max_velocity_vertical_       = 0.0;
+     129             :   double      override_max_acceleration_horizontal_ = 0.0;
+     130             :   double      override_max_acceleration_vertical_   = 0.0;
+     131             :   double      override_max_jerk_horizontal_         = 0.0;
+     132             :   double      override_max_jerk_vertical_           = 0.0;
+     133             : 
+     134             :   // | -------------- variable parameters (deduced) ------------- |
+     135             : 
+     136             :   double     max_execution_time_ = 0;
+     137             :   std::mutex mutex_max_execution_time_;
+     138             : 
+     139             :   bool max_deviation_first_segment_;
+     140             : 
+     141             :   std::atomic<bool> dont_prepend_initial_condition_ = false;
+     142             : 
+     143             :   // | -------------------- the transformer  -------------------- |
+     144             : 
+     145             :   std::shared_ptr<mrs_lib::Transformer> transformer_;
+     146             : 
+     147             :   // | ------------------- scope timer logger ------------------- |
+     148             : 
+     149             :   bool                                       scope_timer_enabled_ = false;
+     150             :   std::shared_ptr<mrs_lib::ScopeTimerLogger> scope_timer_logger_;
+     151             : 
+     152             :   // service client for input
+     153             :   bool               callbackPathSrv(mrs_msgs::PathSrv::Request& req, mrs_msgs::PathSrv::Response& res);
+     154             :   ros::ServiceServer service_server_path_;
+     155             : 
+     156             :   // service client for returning result to the user
+     157             :   bool               callbackGetPathSrv(mrs_msgs::GetPathSrv::Request& req, mrs_msgs::GetPathSrv::Response& res);
+     158             :   ros::ServiceServer service_server_get_path_;
+     159             : 
+     160             :   void                                      callbackPath(const mrs_msgs::Path::ConstPtr msg);
+     161             :   mrs_lib::SubscribeHandler<mrs_msgs::Path> sh_path_;
+     162             : 
+     163             :   mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand>      sh_tracker_cmd_;
+     164             :   mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints> sh_constraints_;
+     165             : 
+     166             :   void                                          callbackUavState(const mrs_msgs::UavState::ConstPtr msg);
+     167             :   mrs_lib::SubscribeHandler<mrs_msgs::UavState> sh_uav_state_;
+     168             : 
+     169             :   mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics> sh_control_manager_diag_;
+     170             : 
+     171             :   // service client for publishing trajectory out
+     172             :   ros::ServiceClient service_client_trajectory_reference_;
+     173             : 
+     174             :   // solve the whole problem
+     175             :   std::tuple<bool, std::string, mrs_msgs::TrajectoryReference> optimize(const std::vector<Waypoint_t>& waypoints_in, const std_msgs::Header& waypoints_stamp,
+     176             :                                                                         bool fallback_sampling, const bool relax_heading);
+     177             : 
+     178             :   std::tuple<std::optional<mrs_msgs::TrackerCommand>, bool, int> prepareInitialCondition(const ros::Time path_time);
+     179             : 
+     180             :   // batch vizualizer
+     181             :   mrs_lib::BatchVisualizer bw_original_;
+     182             :   mrs_lib::BatchVisualizer bw_final_;
+     183             : 
+     184             :   // transforming TrackerCommand
+     185             :   std::optional<mrs_msgs::Path> transformPath(const mrs_msgs::Path& path, const std::string& target_frame);
+     186             : 
+     187             :   // | ------------------ trajectory validation ----------------- |
+     188             : 
+     189             :   /**
+     190             :    * @brief validates samples of a trajectory agains a path of waypoints
+     191             :    *
+     192             :    * @param trajectory
+     193             :    * @param segments
+     194             :    *
+     195             :    * @return <success, traj_fail_idx, path_fail_segment>
+     196             :    */
+     197             :   std::tuple<bool, int, std::vector<bool>, double> validateTrajectorySpatial(const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory,
+     198             :                                                                              const std::vector<Waypoint_t>&                    waypoints);
+     199             : 
+     200             :   std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> findTrajectory(const std::vector<Waypoint_t>&                 waypoints,
+     201             :                                                                            const std::optional<mrs_msgs::TrackerCommand>& initial_state,
+     202             :                                                                            const double& sampling_dt, const bool& relax_heading);
+     203             : 
+     204             :   std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> findTrajectoryFallback(const std::vector<Waypoint_t>& waypoints, const double& sampling_dt,
+     205             :                                                                                    const bool& relax_heading);
+     206             : 
+     207             :   std::future<std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector>> future_trajectory_result_;
+     208             :   std::atomic<bool>                                                      running_async_planning_ = false;
+     209             : 
+     210             :   std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> findTrajectoryAsync(const std::vector<Waypoint_t>&                 waypoints,
+     211             :                                                                                 const std::optional<mrs_msgs::TrackerCommand>& initial_state,
+     212             :                                                                                 const double& sampling_dt, const bool& relax_heading);
+     213             : 
+     214             :   std::vector<Waypoint_t> preprocessPath(const std::vector<Waypoint_t>& waypoints_in);
+     215             : 
+     216             :   mrs_msgs::TrajectoryReference getTrajectoryReference(const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory,
+     217             :                                                        const std::optional<mrs_msgs::TrackerCommand>& initial_condition, const double& sampling_dt);
+     218             : 
+     219             :   Waypoint_t interpolatePoint(const Waypoint_t& a, const Waypoint_t& b, const double& coeff);
+     220             : 
+     221             :   bool checkNaN(const Waypoint_t& a);
+     222             : 
+     223             :   double distFromSegment(const vec3_t& point, const vec3_t& seg1, const vec3_t& seg2);
+     224             : 
+     225             :   bool trajectorySrv(const mrs_msgs::TrajectoryReference& msg);
+     226             : 
+     227             :   // | --------------- dynamic reconfigure server --------------- |
+     228             : 
+     229             :   boost::recursive_mutex                           mutex_drs_;
+     230             :   typedef mrs_uav_trajectory_generation::drsConfig DrsParams_t;
+     231             :   typedef dynamic_reconfigure::Server<DrsParams_t> Drs_t;
+     232             :   boost::shared_ptr<Drs_t>                         drs_;
+     233             :   void                                             callbackDrs(mrs_uav_trajectory_generation::drsConfig& params, uint32_t level);
+     234             :   DrsParams_t                                      params_;
+     235             :   std::mutex                                       mutex_params_;
+     236             : 
+     237             :   // | ------------ Republisher for the desired path ------------ |
+     238             : 
+     239             :   mrs_lib::PublisherHandler<mrs_msgs::Path> ph_original_path_;
+     240             : 
+     241             :   // | ------------- measuring the time of execution ------------ |
+     242             : 
+     243             :   ros::Time  start_time_total_;
+     244             :   std::mutex mutex_start_time_total_;
+     245             :   bool       overtime(void);
+     246             :   double     timeLeft(void);
+     247             : };
+     248             : 
+     249             : //}
+     250             : 
+     251             : /* onInit() //{ */
+     252             : 
+     253         109 : void MrsTrajectoryGeneration::onInit() {
+     254             : 
+     255             :   /* obtain node handle */
+     256         109 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     257             : 
+     258             :   /* waits for the ROS to publish clock */
+     259         109 :   ros::Time::waitForValid();
+     260             : 
+     261             :   // | ----------------------- publishers ----------------------- |
+     262             : 
+     263         109 :   ph_original_path_ = mrs_lib::PublisherHandler<mrs_msgs::Path>(nh_, "original_path_out", 1);
+     264             : 
+     265             :   // | ----------------------- subscribers ---------------------- |
+     266             : 
+     267         218 :   mrs_lib::SubscribeHandlerOptions shopts;
+     268         109 :   shopts.nh                 = nh_;
+     269         109 :   shopts.node_name          = "TrajectoryGeneration";
+     270         109 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     271         109 :   shopts.threadsafe         = true;
+     272         109 :   shopts.autostart          = true;
+     273         109 :   shopts.queue_size         = 10;
+     274         109 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     275             : 
+     276         109 :   sh_constraints_          = mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints>(shopts, "constraints_in");
+     277         109 :   sh_tracker_cmd_          = mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand>(shopts, "tracker_cmd_in");
+     278         109 :   sh_uav_state_            = mrs_lib::SubscribeHandler<mrs_msgs::UavState>(shopts, "uav_state_in", &MrsTrajectoryGeneration::callbackUavState, this);
+     279         109 :   sh_control_manager_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>(shopts, "control_manager_diag_in");
+     280             : 
+     281         109 :   sh_path_ = mrs_lib::SubscribeHandler<mrs_msgs::Path>(shopts, "path_in", &MrsTrajectoryGeneration::callbackPath, this);
+     282             : 
+     283             :   // | --------------------- service servers -------------------- |
+     284             : 
+     285         109 :   service_server_path_ = nh_.advertiseService("path_in", &MrsTrajectoryGeneration::callbackPathSrv, this);
+     286             : 
+     287         109 :   service_server_get_path_ = nh_.advertiseService("get_path_in", &MrsTrajectoryGeneration::callbackGetPathSrv, this);
+     288             : 
+     289         109 :   service_client_trajectory_reference_ = nh_.serviceClient<mrs_msgs::TrajectoryReferenceSrv>("trajectory_reference_out");
+     290             : 
+     291             :   // | ----------------------- parameters ----------------------- |
+     292             : 
+     293         218 :   mrs_lib::ParamLoader param_loader(nh_, "MrsTrajectoryGeneration");
+     294             : 
+     295         218 :   std::string custom_config_path;
+     296         218 :   std::string platform_config_path;
+     297         218 :   std::string uav_manager_config_path;
+     298             : 
+     299         109 :   param_loader.loadParam("custom_config", custom_config_path);
+     300         109 :   param_loader.loadParam("platform_config", platform_config_path);
+     301         109 :   param_loader.loadParam("uav_manager_config", uav_manager_config_path);
+     302             : 
+     303         109 :   if (uav_manager_config_path == "") {
+     304           0 :     ROS_ERROR("[MrsTrajectoryGeneration]: uav_manager_config param is empty");
+     305           0 :     ros::shutdown();
+     306             :   }
+     307             : 
+     308         109 :   if (custom_config_path != "") {
+     309         109 :     param_loader.addYamlFile(custom_config_path);
+     310             :   }
+     311             : 
+     312         109 :   if (platform_config_path != "") {
+     313         109 :     param_loader.addYamlFile(platform_config_path);
+     314             :   }
+     315             : 
+     316         109 :   param_loader.addYamlFile(uav_manager_config_path);
+     317             : 
+     318         109 :   param_loader.addYamlFileFromParam("private_config");
+     319         109 :   param_loader.addYamlFileFromParam("public_config");
+     320             : 
+     321         218 :   const std::string yaml_prefix = "mrs_uav_trajectory_generation/";
+     322             : 
+     323         109 :   param_loader.loadParam("uav_name", _uav_name_);
+     324             : 
+     325         109 :   param_loader.loadParam(yaml_prefix + "sampling_dt", _sampling_dt_);
+     326             : 
+     327         109 :   param_loader.loadParam(yaml_prefix + "enforce_fallback_solver", params_.enforce_fallback_solver);
+     328             : 
+     329         109 :   param_loader.loadParam(yaml_prefix + "max_trajectory_len_factor", _max_trajectory_len_factor_);
+     330         109 :   param_loader.loadParam(yaml_prefix + "min_trajectory_len_factor", _min_trajectory_len_factor_);
+     331             : 
+     332         109 :   param_loader.loadParam(yaml_prefix + "n_attempts", _n_attempts_);
+     333         109 :   param_loader.loadParam(yaml_prefix + "fallback_sampling/enabled", _fallback_sampling_enabled_);
+     334         109 :   param_loader.loadParam(yaml_prefix + "fallback_sampling/speed_factor", _fallback_sampling_speed_factor_);
+     335         109 :   param_loader.loadParam(yaml_prefix + "fallback_sampling/accel_factor", _fallback_sampling_accel_factor_);
+     336         109 :   param_loader.loadParam(yaml_prefix + "fallback_sampling/stopping_time", _fallback_sampling_stopping_time_);
+     337         109 :   param_loader.loadParam(yaml_prefix + "fallback_sampling/first_waypoint_additional_stop", _fallback_sampling_first_waypoint_additional_stop_);
+     338             : 
+     339         109 :   param_loader.loadParam(yaml_prefix + "check_trajectory_deviation/enabled", _trajectory_max_segment_deviation_enabled_);
+     340         109 :   param_loader.loadParam(yaml_prefix + "check_trajectory_deviation/max_deviation", params_.max_deviation);
+     341         109 :   param_loader.loadParam(yaml_prefix + "check_trajectory_deviation/max_iterations", _trajectory_max_segment_deviation_max_iterations_);
+     342             : 
+     343         109 :   param_loader.loadParam(yaml_prefix + "path_straightener/enabled", _path_straightener_enabled_);
+     344         109 :   param_loader.loadParam(yaml_prefix + "path_straightener/max_deviation", _path_straightener_max_deviation_);
+     345         109 :   param_loader.loadParam(yaml_prefix + "path_straightener/max_hdg_deviation", _path_straightener_max_hdg_deviation_);
+     346             : 
+     347         109 :   param_loader.loadParam(yaml_prefix + "override_heading_atan2", _override_heading_atan2_);
+     348             : 
+     349         109 :   param_loader.loadParam(yaml_prefix + "min_waypoint_distance", _min_waypoint_distance_);
+     350             : 
+     351         109 :   param_loader.loadParam("mrs_uav_managers/uav_manager/takeoff/takeoff_height", _takeoff_height_);
+     352             : 
+     353             :   // | --------------------- tf transformer --------------------- |
+     354             : 
+     355         109 :   transformer_ = std::make_shared<mrs_lib::Transformer>(nh_, "TrajectoryGeneration");
+     356         109 :   transformer_->setDefaultPrefix(_uav_name_);
+     357         109 :   transformer_->retryLookupNewest(true);
+     358             : 
+     359             :   // | ------------------- scope timer logger ------------------- |
+     360             : 
+     361         109 :   param_loader.loadParam(yaml_prefix + "scope_timer/enabled", scope_timer_enabled_);
+     362         327 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+     363         109 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+     364             : 
+     365             :   // | --------------------- service clients -------------------- |
+     366             : 
+     367         109 :   param_loader.loadParam(yaml_prefix + "time_penalty", params_.time_penalty);
+     368         109 :   param_loader.loadParam(yaml_prefix + "soft_constraints_enabled", params_.soft_constraints_enabled);
+     369         109 :   param_loader.loadParam(yaml_prefix + "soft_constraints_weight", params_.soft_constraints_weight);
+     370         109 :   param_loader.loadParam(yaml_prefix + "time_allocation", params_.time_allocation);
+     371         109 :   param_loader.loadParam(yaml_prefix + "equality_constraint_tolerance", params_.equality_constraint_tolerance);
+     372         109 :   param_loader.loadParam(yaml_prefix + "inequality_constraint_tolerance", params_.inequality_constraint_tolerance);
+     373         109 :   param_loader.loadParam(yaml_prefix + "max_iterations", params_.max_iterations);
+     374         109 :   param_loader.loadParam(yaml_prefix + "derivative_to_optimize", params_.derivative_to_optimize);
+     375             : 
+     376         109 :   param_loader.loadParam(yaml_prefix + "max_time", params_.max_execution_time);
+     377             : 
+     378         109 :   max_execution_time_ = params_.max_execution_time;
+     379             : 
+     380         109 :   if (!param_loader.loadedSuccessfully()) {
+     381           0 :     ROS_ERROR("[TrajectoryGeneration]: could not load all parameters!");
+     382           0 :     ros::shutdown();
+     383             :   }
+     384             : 
+     385             :   // | -------------------- batch visualizer -------------------- |
+     386             : 
+     387         109 :   bw_original_ = mrs_lib::BatchVisualizer(nh_, "markers/original", "");
+     388             : 
+     389         109 :   bw_original_.clearBuffers();
+     390         109 :   bw_original_.clearVisuals();
+     391             : 
+     392         109 :   bw_final_ = mrs_lib::BatchVisualizer(nh_, "markers/final", "");
+     393             : 
+     394         109 :   bw_final_.clearBuffers();
+     395         109 :   bw_final_.clearVisuals();
+     396             : 
+     397             :   // | --------------- dynamic reconfigure server --------------- |
+     398             : 
+     399         109 :   drs_.reset(new Drs_t(mutex_drs_, nh_));
+     400         109 :   drs_->updateConfig(params_);
+     401         109 :   Drs_t::CallbackType f = boost::bind(&MrsTrajectoryGeneration::callbackDrs, this, _1, _2);
+     402         109 :   drs_->setCallback(f);
+     403             : 
+     404             :   // | --------------------- finish the init -------------------- |
+     405             : 
+     406         109 :   ROS_INFO_ONCE("[TrajectoryGeneration]: initialized");
+     407             : 
+     408         109 :   is_initialized_ = true;
+     409         109 : }
+     410             : 
+     411             : //}
+     412             : 
+     413             : // | ---------------------- main routines --------------------- |
+     414             : 
+     415             : /*
+     416             :  * 1. preprocessPath(): preprocessing the incoming path
+     417             :  *    - throughs away too close waypoints
+     418             :  *    - straightness path by neglecting waypoints close to segments
+     419             :  * 2. optimize(): solves the whole problem including
+     420             :  *    - subdivision for satisfying max deviation
+     421             :  * 3. findTrajectory(): solves single instance by the ETH tool
+     422             :  * 4. findTrajectoryFallback(): Baca's sampling for backup solution
+     423             :  * 5. validateTrajectorySpatial(): checks for the spatial soundness of a trajectory vs. the original path
+     424             :  */
+     425             : 
+     426             : /* preprocessPath() //{ */
+     427             : 
+     428          11 : std::vector<Waypoint_t> MrsTrajectoryGeneration::preprocessPath(const std::vector<Waypoint_t>& waypoints_in) {
+     429             : 
+     430          33 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::preprocessPath", scope_timer_logger_, scope_timer_enabled_);
+     431             : 
+     432          11 :   std::vector<Waypoint_t> waypoints;
+     433             : 
+     434          11 :   size_t last_added_idx = 0;  // in "waypoints_in"
+     435             : 
+     436          66 :   for (size_t i = 0; i < waypoints_in.size(); i++) {
+     437             : 
+     438          55 :     double x       = waypoints_in.at(i).coords(0);
+     439          55 :     double y       = waypoints_in.at(i).coords(1);
+     440          55 :     double z       = waypoints_in.at(i).coords(2);
+     441          55 :     double heading = waypoints_in.at(i).coords(3);
+     442             : 
+     443          55 :     bw_original_.addPoint(vec3_t(x, y, z), 1.0, 0.0, 0.0, 1.0);
+     444             : 
+     445          55 :     if (_path_straightener_enabled_ && waypoints_in.size() >= 3 && i > 0 && i < (waypoints_in.size() - 1)) {
+     446             : 
+     447           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));
+     448           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));
+     449             : 
+     450           0 :       double first_hdg = waypoints_in.at(last_added_idx).coords(3);
+     451           0 :       double last_hdg  = waypoints_in.at(i + 1).coords(3);
+     452             : 
+     453           0 :       size_t next_point = last_added_idx + 1;
+     454             : 
+     455           0 :       bool segment_is_ok = true;
+     456             : 
+     457           0 :       for (size_t j = next_point; j < i + 1; j++) {
+     458             : 
+     459           0 :         vec3_t mid(waypoints_in.at(j).coords(0), waypoints_in.at(j).coords(1), waypoints_in.at(j).coords(2));
+     460           0 :         double mid_hdg = waypoints_in.at(j).coords(3);
+     461             : 
+     462           0 :         double dist_from_segment = distFromSegment(mid, first, last);
+     463             : 
+     464           0 :         if (dist_from_segment > _path_straightener_max_deviation_ || fabs(radians::diff(first_hdg, mid_hdg) > _path_straightener_max_hdg_deviation_) ||
+     465           0 :             fabs(radians::diff(last_hdg, mid_hdg) > _path_straightener_max_hdg_deviation_)) {
+     466           0 :           segment_is_ok = false;
+     467           0 :           break;
+     468             :         }
+     469             :       }
+     470             : 
+     471           0 :       if (segment_is_ok) {
+     472           0 :         continue;
+     473             :       }
+     474             :     }
+     475             : 
+     476          55 :     if (i > 0 && i < (waypoints_in.size() - 1)) {
+     477             : 
+     478          33 :       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));
+     479          33 :       vec3_t last(waypoints_in.at(i).coords(0), waypoints_in.at(i).coords(1), waypoints_in.at(i).coords(2));
+     480             : 
+     481          33 :       if (mrs_lib::geometry::dist(first, last) < _min_waypoint_distance_) {
+     482           0 :         ROS_INFO("[TrajectoryGeneration]: waypoint #%d too close (< %.3f m) to the previous one (#%d), throwing it away", int(i), _min_waypoint_distance_,
+     483             :                  int(last_added_idx));
+     484           0 :         continue;
+     485             :       }
+     486             :     }
+     487             : 
+     488          55 :     Waypoint_t wp;
+     489          55 :     wp.coords  = Eigen::Vector4d(x, y, z, heading);
+     490          55 :     wp.stop_at = waypoints_in.at(i).stop_at;
+     491          55 :     waypoints.push_back(wp);
+     492             : 
+     493          55 :     last_added_idx = i;
+     494             :   }
+     495             : 
+     496          22 :   return waypoints;
+     497             : }
+     498             : 
+     499             : //}
+     500             : 
+     501             : /* prepareInitialCondition() //{ */
+     502             : 
+     503          11 : std::tuple<std::optional<mrs_msgs::TrackerCommand>, bool, int> MrsTrajectoryGeneration::prepareInitialCondition(const ros::Time path_time) {
+     504             : 
+     505          11 :   if (dont_prepend_initial_condition_) {
+     506           0 :     return {{}, false, 0};
+     507             :   }
+     508             : 
+     509          22 :   auto tracker_cmd = sh_tracker_cmd_.getMsg();
+     510             : 
+     511             :   // | ------------- prepare the initial conditions ------------- |
+     512             : 
+     513          22 :   mrs_msgs::TrackerCommand initial_condition;
+     514             : 
+     515          11 :   if (!sh_tracker_cmd_.hasMsg() || ((ros::Time::now() - sh_tracker_cmd_.lastMsgTime())).toSec() > 1.0) {
+     516             : 
+     517           4 :     auto uav_state = sh_uav_state_.getMsg();
+     518             : 
+     519           4 :     initial_condition.position = uav_state->pose.position;
+     520             : 
+     521             :     try {
+     522           4 :       initial_condition.heading = mrs_lib::AttitudeConverter(uav_state->pose.orientation).getHeading();
+     523             :     }
+     524           0 :     catch (...) {
+     525           0 :       ROS_WARN_THROTTLE(1.0, "[MrsTrajectoryGeneration]: could not obtain heading from the UAV State");
+     526             :     }
+     527             : 
+     528           4 :     initial_condition.position.z += _takeoff_height_;
+     529             : 
+     530           4 :     initial_condition.header = uav_state->header;
+     531             : 
+     532           8 :     return {initial_condition, false, 0};
+     533             :   }
+     534             : 
+     535           7 :   bool path_from_future = false;
+     536             : 
+     537             :   // positive = in the future
+     538           7 :   double path_time_offset = 0;
+     539             : 
+     540           7 :   if (path_time != ros::Time(0)) {
+     541           0 :     path_time_offset = (path_time - ros::Time::now()).toSec();
+     542             :   }
+     543             : 
+     544           7 :   int path_sample_offset = 0;
+     545             : 
+     546             :   // if the desired path starts in the future, more than one MPC step ahead
+     547           7 :   if (path_time_offset > 0.2) {
+     548             : 
+     549           0 :     ROS_INFO("[TrajectoryGeneration]: desired path is from the future by %.2f s", path_time_offset);
+     550             : 
+     551             :     // calculate the offset in samples in the predicted trajectory
+     552             :     // 0.01 is subtracted for the first sample, which is smaller
+     553             :     // +1 is added due to the first sample, which was subtarcted
+     554           0 :     path_sample_offset = int(ceil((path_time_offset * FUTURIZATION_FIRST_WAYPOINT_FACTOR - 0.01) / 0.2)) + 1;
+     555             : 
+     556           0 :     if (path_sample_offset > (int(tracker_cmd->full_state_prediction.position.size()) - 1)) {
+     557             : 
+     558           0 :       ROS_ERROR("[TrajectoryGeneration]: can not extrapolate into the waypoints, using tracker_cmd instead");
+     559           0 :       initial_condition = *tracker_cmd;
+     560             : 
+     561             :     } else {
+     562             : 
+     563             :       // copy the sample from the current prediction into TrackerCommand, so that we can easily transform it
+     564           0 :       mrs_msgs::TrackerCommand full_state;
+     565             : 
+     566           0 :       full_state.header = tracker_cmd->full_state_prediction.header;
+     567             : 
+     568           0 :       full_state.position     = tracker_cmd->full_state_prediction.position.at(path_sample_offset);
+     569           0 :       full_state.velocity     = tracker_cmd->full_state_prediction.velocity.at(path_sample_offset);
+     570           0 :       full_state.acceleration = tracker_cmd->full_state_prediction.acceleration.at(path_sample_offset);
+     571           0 :       full_state.jerk         = tracker_cmd->full_state_prediction.jerk.at(path_sample_offset);
+     572             : 
+     573           0 :       full_state.heading              = tracker_cmd->full_state_prediction.heading.at(path_sample_offset);
+     574           0 :       full_state.heading_rate         = tracker_cmd->full_state_prediction.heading_rate.at(path_sample_offset);
+     575           0 :       full_state.heading_acceleration = tracker_cmd->full_state_prediction.heading_acceleration.at(path_sample_offset);
+     576           0 :       full_state.heading_jerk         = tracker_cmd->full_state_prediction.heading_jerk.at(path_sample_offset);
+     577             : 
+     578           0 :       ROS_INFO("[TrajectoryGeneration]: getting initial condition from the %d-th sample of the MPC prediction", path_sample_offset);
+     579             : 
+     580           0 :       initial_condition.header = full_state.header;
+     581             : 
+     582           0 :       initial_condition.position     = full_state.position;
+     583           0 :       initial_condition.velocity     = full_state.velocity;
+     584           0 :       initial_condition.acceleration = full_state.acceleration;
+     585           0 :       initial_condition.jerk         = full_state.jerk;
+     586             : 
+     587           0 :       initial_condition.heading              = full_state.heading;
+     588           0 :       initial_condition.heading_rate         = full_state.heading_rate;
+     589           0 :       initial_condition.heading_acceleration = full_state.heading_acceleration;
+     590           0 :       initial_condition.heading_jerk         = full_state.heading_jerk;
+     591             : 
+     592           0 :       path_from_future = true;
+     593             :     }
+     594             : 
+     595             :   } else {
+     596             : 
+     597           7 :     ROS_INFO("[TrajectoryGeneration]: desired path is NOT from the future, using tracker_cmd as the initial condition");
+     598             : 
+     599           7 :     initial_condition = *tracker_cmd;
+     600             :   }
+     601             : 
+     602           7 :   auto control_manager_diag = sh_control_manager_diag_.getMsg();
+     603             : 
+     604           7 :   if (path_time == ros::Time(0)) {
+     605           7 :     if (!control_manager_diag->tracker_status.have_goal) {
+     606           3 :       initial_condition.header.stamp = ros::Time(0);
+     607             :     }
+     608             :   }
+     609             : 
+     610           7 :   return {{initial_condition}, path_from_future, path_sample_offset};
+     611             : }
+     612             : 
+     613             : //}
+     614             : 
+     615             : /* optimize() //{ */
+     616             : 
+     617          11 : std::tuple<bool, std::string, mrs_msgs::TrajectoryReference> MrsTrajectoryGeneration::optimize(const std::vector<Waypoint_t>& waypoints_in,
+     618             :                                                                                                const std_msgs::Header&        waypoints_header,
+     619             :                                                                                                const bool fallback_sampling, const bool relax_heading) {
+     620             : 
+     621          33 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::optimize", scope_timer_logger_, scope_timer_enabled_);
+     622             : 
+     623          11 :   ros::Time optimize_time_start = ros::Time::now();
+     624             : 
+     625             :   // | ---------------- reset the visual markers ---------------- |
+     626             : 
+     627          11 :   bw_original_.clearBuffers();
+     628          11 :   bw_original_.clearVisuals();
+     629          11 :   bw_final_.clearBuffers();
+     630          11 :   bw_final_.clearVisuals();
+     631             : 
+     632          11 :   bw_original_.setParentFrame(transformer_->resolveFrame(frame_id_));
+     633          11 :   bw_final_.setParentFrame(transformer_->resolveFrame(frame_id_));
+     634             : 
+     635          11 :   bw_original_.setPointsScale(0.4);
+     636          11 :   bw_final_.setPointsScale(0.35);
+     637             : 
+     638             :   // empty path is invalid
+     639          11 :   if (waypoints_in.size() == 0) {
+     640           0 :     std::stringstream ss;
+     641           0 :     ss << "the path is empty (before postprocessing)";
+     642           0 :     ROS_ERROR_STREAM("[TrajectoryGeneration]: " << ss.str());
+     643           0 :     return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference());
+     644             :   }
+     645             : 
+     646          22 :   std::vector<Waypoint_t> waypoints_in_with_init = waypoints_in;
+     647             : 
+     648          11 :   double path_time_offset = (waypoints_header.stamp - ros::Time::now()).toSec();
+     649             : 
+     650          11 :   if (path_time_offset > 0.2 && waypoints_in_with_init.size() >= 2) {
+     651           0 :     waypoints_in_with_init.erase(waypoints_in_with_init.begin());
+     652             :   }
+     653             : 
+     654          22 :   auto [initial_condition, path_from_future, path_sample_offset] = prepareInitialCondition(waypoints_header.stamp);
+     655             : 
+     656             :   // prepend the initial condition
+     657          11 :   if (initial_condition) {
+     658             : 
+     659          11 :     Waypoint_t initial_waypoint;
+     660             :     initial_waypoint.coords =
+     661          11 :         Eigen::Vector4d(initial_condition->position.x, initial_condition->position.y, initial_condition->position.z, initial_condition->heading);
+     662          11 :     initial_waypoint.stop_at = false;
+     663          11 :     waypoints_in_with_init.insert(waypoints_in_with_init.begin(), initial_waypoint);
+     664             : 
+     665             :   } else {
+     666           0 :     if (!dont_prepend_initial_condition_) {
+     667           0 :       fly_now_ = false;
+     668             :     }
+     669             :   }
+     670             : 
+     671          22 :   std::vector<Waypoint_t> waypoints = preprocessPath(waypoints_in_with_init);
+     672             : 
+     673          11 :   if (waypoints.size() <= 1) {
+     674           0 :     std::stringstream ss;
+     675           0 :     ss << "the path is empty (after postprocessing)";
+     676           0 :     ROS_ERROR_STREAM("[TrajectoryGeneration]: " << ss.str());
+     677           0 :     return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference());
+     678             :   }
+     679             : 
+     680          11 :   bool              safe = false;
+     681             :   int               traj_idx;
+     682          22 :   std::vector<bool> segment_safeness;
+     683          11 :   double            max_deviation = 0;
+     684             : 
+     685          22 :   eth_mav_msgs::EigenTrajectoryPoint::Vector trajectory;
+     686             : 
+     687          11 :   double sampling_dt = 0;
+     688             : 
+     689          11 :   if (path_from_future) {
+     690           0 :     ROS_INFO("[TrajectoryGeneration]: changing dt = 0.2, cause the path is from the future");
+     691           0 :     sampling_dt = 0.2;
+     692             :   } else {
+     693          11 :     sampling_dt = _sampling_dt_;
+     694             :   }
+     695             : 
+     696          11 :   std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> result;
+     697             : 
+     698          22 :   auto params = mrs_lib::get_mutexed(mutex_params_, params_);
+     699             : 
+     700          11 :   if (params.enforce_fallback_solver) {
+     701           0 :     ROS_WARN("[TrajectoryGeneration]: fallback sampling enforced");
+     702           0 :     result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading);
+     703          11 :   } else if (fallback_sampling) {
+     704           0 :     ROS_WARN("[TrajectoryGeneration]: executing fallback sampling");
+     705           0 :     result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading);
+     706          11 :   } else if (running_async_planning_) {
+     707           5 :     ROS_WARN("[TrajectoryGeneration]: executing fallback sampling, the previous async task is still running");
+     708           5 :     result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading);
+     709           6 :   } else if (overtime()) {
+     710           1 :     ROS_WARN("[TrajectoryGeneration]: executing fallback sampling, we are running over time");
+     711           1 :     result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading);
+     712             :   } else {
+     713             : 
+     714           5 :     result = findTrajectoryAsync(waypoints, initial_condition, sampling_dt, relax_heading);
+     715             :   }
+     716             : 
+     717          11 :   if (result) {
+     718          11 :     trajectory = result.value();
+     719             :   } else {
+     720           0 :     std::stringstream ss;
+     721           0 :     ss << "failed to find trajectory";
+     722           0 :     ROS_ERROR_STREAM("[TrajectoryGeneration]: " << ss.str());
+     723           0 :     return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference());
+     724             :   }
+     725             : 
+     726          17 :   for (int k = 0; k < _trajectory_max_segment_deviation_max_iterations_; k++) {
+     727             : 
+     728          17 :     ROS_DEBUG("[TrajectoryGeneration]: revalidation cycle #%d", k);
+     729             : 
+     730          17 :     std::tie(safe, traj_idx, segment_safeness, max_deviation) = validateTrajectorySpatial(trajectory, waypoints);
+     731             : 
+     732          17 :     if (_trajectory_max_segment_deviation_enabled_ && !safe) {
+     733             : 
+     734          11 :       ROS_DEBUG("[TrajectoryGeneration]: trajectory is not safe, max deviation %.3f m", max_deviation);
+     735             : 
+     736          11 :       std::vector<Waypoint_t>::iterator waypoint = waypoints.begin();
+     737          11 :       std::vector<bool>::iterator       safeness = segment_safeness.begin();
+     738             : 
+     739         111 :       for (; waypoint < waypoints.end() - 1; waypoint++) {
+     740             : 
+     741         100 :         if (!(*safeness)) {
+     742             : 
+     743          51 :           if (waypoint > waypoints.begin() || max_deviation_first_segment_ || int(waypoints.size()) <= 2) {
+     744          51 :             Waypoint_t midpoint2 = interpolatePoint(*waypoint, *(waypoint + 1), 0.5);
+     745          51 :             waypoint             = waypoints.insert(waypoint + 1, midpoint2);
+     746             :           }
+     747             :         }
+     748             : 
+     749         100 :         safeness++;
+     750             :       }
+     751             : 
+     752          11 :       if (params.enforce_fallback_solver) {
+     753           0 :         ROS_WARN("[TrajectoryGeneration]: fallback sampling enforced");
+     754           0 :         result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading);
+     755          11 :       } else if (fallback_sampling) {
+     756           0 :         ROS_WARN("[TrajectoryGeneration]: executing fallback sampling");
+     757           0 :         result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading);
+     758          11 :       } else if (running_async_planning_) {
+     759           0 :         ROS_WARN("[TrajectoryGeneration]: executing fallback sampling, the previous async task is still running");
+     760           0 :         result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading);
+     761          11 :       } else if (overtime()) {
+     762           0 :         ROS_WARN("[TrajectoryGeneration]: executing fallback sampling, we are running over time");
+     763           0 :         result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading);
+     764             :       } else {
+     765          11 :         result = findTrajectoryAsync(waypoints, initial_condition, sampling_dt, relax_heading);
+     766             :       }
+     767             : 
+     768          11 :       if (result) {
+     769           6 :         trajectory = result.value();
+     770             :       } else {
+     771           5 :         std::stringstream ss;
+     772           5 :         ss << "failed to find trajectory";
+     773           5 :         ROS_WARN_STREAM("[TrajectoryGeneration]: " << ss.str());
+     774           5 :         return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference());
+     775           6 :       }
+     776             : 
+     777             :     } else {
+     778           6 :       ROS_DEBUG("[TrajectoryGeneration]: trajectory is safe (%.2f)", max_deviation);
+     779           6 :       safe = true;
+     780           6 :       break;
+     781             :     }
+     782             :   }
+     783             : 
+     784           6 :   ROS_INFO("[TrajectoryGeneration]: final max trajectory-path deviation: %.2f m, total trajectory time: %.2fs ", max_deviation,
+     785             :            trajectory.size() * sampling_dt);
+     786             : 
+     787             :   // prepare rviz markers
+     788          36 :   for (int i = 0; i < int(waypoints.size()); i++) {
+     789          30 :     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);
+     790             :   }
+     791             : 
+     792          12 :   mrs_msgs::TrajectoryReference mrs_trajectory;
+     793             : 
+     794             :   // convert the optimized trajectory to mrs_msgs::TrajectoryReference
+     795           6 :   mrs_trajectory = getTrajectoryReference(trajectory, initial_condition, sampling_dt);
+     796             : 
+     797             :   // insert part of the MPC prediction in the front of the generated trajectory to compensate for the future
+     798           6 :   if (path_from_future) {
+     799             : 
+     800           0 :     auto current_prediction = sh_tracker_cmd_.getMsg()->full_state_prediction;
+     801             : 
+     802             :     // calculate the starting idx that we will use from the current_prediction
+     803           0 :     double path_time_offset_2   = (ros::Time::now() - current_prediction.header.stamp).toSec();  // = how long did it take to optimize
+     804           0 :     int    path_sample_offset_2 = int(floor((path_time_offset_2 - 0.01) / 0.2)) + 1;
+     805             : 
+     806             :     // if there is anything to insert
+     807           0 :     if (path_sample_offset > path_sample_offset_2) {
+     808             : 
+     809           0 :       ROS_INFO("[TrajectoryGeneration]: inserting pre-trajectory from the prediction, idxs %d to %d", path_sample_offset_2, path_sample_offset);
+     810             : 
+     811           0 :       for (int i = path_sample_offset - 1; i >= 0; i--) {
+     812             : 
+     813           0 :         ROS_DEBUG("[TrajectoryGeneration]: inserting idx %d", i);
+     814             : 
+     815           0 :         mrs_msgs::ReferenceStamped reference;
+     816             : 
+     817           0 :         reference.header = current_prediction.header;
+     818             : 
+     819           0 :         reference.reference.heading = current_prediction.heading.at(i);
+     820             : 
+     821           0 :         reference.reference.position = current_prediction.position.at(i);
+     822             : 
+     823           0 :         auto res = transformer_->transformSingle(reference, waypoints_header.frame_id);
+     824             : 
+     825           0 :         if (res) {
+     826           0 :           reference = res.value();
+     827             :         } else {
+     828           0 :           std::stringstream ss;
+     829           0 :           ss << "could not transform reference to the path frame";
+     830           0 :           ROS_ERROR_STREAM("[TrajectoryGeneration]: " << ss.str());
+     831           0 :           return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference());
+     832             :         }
+     833             : 
+     834           0 :         mrs_trajectory.points.insert(mrs_trajectory.points.begin(), reference.reference);
+     835             :       }
+     836             :     }
+     837             :   }
+     838             : 
+     839           6 :   bw_original_.publish();
+     840           6 :   bw_final_.publish();
+     841             : 
+     842           6 :   std::stringstream ss;
+     843           6 :   ss << "trajectory generated";
+     844             : 
+     845           6 :   ROS_DEBUG("[TrajectoryGeneration]: trajectory generated, took %.3f s", (ros::Time::now() - optimize_time_start).toSec());
+     846             : 
+     847           6 :   return std::tuple(true, ss.str(), mrs_trajectory);
+     848             : }
+     849             : 
+     850             : //}
+     851             : 
+     852             : /* findTrajectory() //{ */
+     853             : 
+     854          16 : std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> MrsTrajectoryGeneration::findTrajectory(const std::vector<Waypoint_t>&                 waypoints,
+     855             :                                                                                                   const std::optional<mrs_msgs::TrackerCommand>& initial_state,
+     856             :                                                                                                   const double& sampling_dt, const bool& relax_heading) {
+     857             : 
+     858          48 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::findTrajectory", scope_timer_logger_, scope_timer_enabled_);
+     859             : 
+     860          32 :   mrs_lib::AtomicScopeFlag unset_running(running_async_planning_);
+     861             : 
+     862          16 :   ROS_DEBUG("[TrajectoryGeneration]: findTrajectory() started");
+     863             : 
+     864          16 :   ros::Time find_trajectory_time_start = ros::Time::now();
+     865             : 
+     866          32 :   auto params      = mrs_lib::get_mutexed(mutex_params_, params_);
+     867          32 :   auto constraints = sh_constraints_.getMsg();
+     868             : 
+     869          32 :   auto control_manager_diag = sh_control_manager_diag_.getMsg();
+     870             : 
+     871          16 :   if (initial_state && (initial_state->header.stamp - ros::Time::now()).toSec() < 0.2 && control_manager_diag->tracker_status.have_goal) {
+     872           3 :     max_deviation_first_segment_ = false;
+     873             :   } else {
+     874          13 :     max_deviation_first_segment_ = true;
+     875             :   }
+     876             : 
+     877             :   // optimizer
+     878             : 
+     879          16 :   eth_trajectory_generation::NonlinearOptimizationParameters parameters;
+     880             : 
+     881          16 :   parameters.f_rel                  = 0.05;
+     882          16 :   parameters.x_rel                  = 0.1;
+     883          16 :   parameters.time_penalty           = params.time_penalty;
+     884          16 :   parameters.use_soft_constraints   = params.soft_constraints_enabled;
+     885          16 :   parameters.soft_constraint_weight = params.soft_constraints_weight;
+     886          16 :   parameters.time_alloc_method      = static_cast<eth_trajectory_generation::NonlinearOptimizationParameters::TimeAllocMethod>(params.time_allocation);
+     887          16 :   if (params.time_allocation == 2) {
+     888          16 :     parameters.algorithm = nlopt::LD_LBFGS;
+     889             :   }
+     890          16 :   parameters.initial_stepsize_rel            = 0.1;
+     891          16 :   parameters.inequality_constraint_tolerance = params.inequality_constraint_tolerance;
+     892          16 :   parameters.equality_constraint_tolerance   = params.equality_constraint_tolerance;
+     893          16 :   parameters.max_iterations                  = params.max_iterations;
+     894             : 
+     895             :   // let's tell the solver tha it is more time then it thinks, to not stop prematurely
+     896          16 :   parameters.max_time = 2 * (NLOPT_EXEC_TIME_FACTOR * timeLeft());
+     897             : 
+     898          32 :   eth_trajectory_generation::Vertex::Vector vertices;
+     899          16 :   const int                                 dimension = 4;
+     900             : 
+     901          16 :   int derivative_to_optimize = eth_trajectory_generation::derivative_order::ACCELERATION;
+     902             : 
+     903          16 :   switch (params.derivative_to_optimize) {
+     904          16 :     case 0: {
+     905          16 :       derivative_to_optimize = eth_trajectory_generation::derivative_order::ACCELERATION;
+     906          16 :       break;
+     907             :     }
+     908           0 :     case 1: {
+     909           0 :       derivative_to_optimize = eth_trajectory_generation::derivative_order::JERK;
+     910           0 :       break;
+     911             :     }
+     912           0 :     case 2: {
+     913           0 :       derivative_to_optimize = eth_trajectory_generation::derivative_order::SNAP;
+     914           0 :       break;
+     915             :     }
+     916             :   }
+     917             : 
+     918             :   // | --------------- add constraints to vertices -------------- |
+     919             : 
+     920             :   double last_heading;
+     921             : 
+     922          16 :   if (initial_state) {
+     923          16 :     last_heading = initial_state->heading;
+     924             :   } else {
+     925           0 :     last_heading = waypoints.at(0).coords(3);
+     926             :   }
+     927             : 
+     928         203 :   for (size_t i = 0; i < waypoints.size(); i++) {
+     929         187 :     double x       = waypoints.at(i).coords(0);
+     930         187 :     double y       = waypoints.at(i).coords(1);
+     931         187 :     double z       = waypoints.at(i).coords(2);
+     932         187 :     double heading = sradians::unwrap(waypoints.at(i).coords(3), last_heading);
+     933         187 :     last_heading   = heading;
+     934             : 
+     935         374 :     eth_trajectory_generation::Vertex vertex(dimension);
+     936             : 
+     937         187 :     if (i == 0) {
+     938             : 
+     939          16 :       vertex.makeStartOrEnd(Eigen::Vector4d(x, y, z, heading), derivative_to_optimize);
+     940             : 
+     941          16 :       vertex.addConstraint(eth_trajectory_generation::derivative_order::POSITION, Eigen::Vector4d(x, y, z, heading));
+     942             : 
+     943          16 :       if (initial_state) {
+     944             : 
+     945          16 :         vertex.addConstraint(eth_trajectory_generation::derivative_order::VELOCITY,
+     946          16 :                              Eigen::Vector4d(initial_state->velocity.x, initial_state->velocity.y, initial_state->velocity.z, initial_state->heading_rate));
+     947             : 
+     948          16 :         vertex.addConstraint(
+     949             :             eth_trajectory_generation::derivative_order::ACCELERATION,
+     950          16 :             Eigen::Vector4d(initial_state->acceleration.x, initial_state->acceleration.y, initial_state->acceleration.z, initial_state->heading_acceleration));
+     951             : 
+     952          16 :         vertex.addConstraint(eth_trajectory_generation::derivative_order::JERK,
+     953          32 :                              Eigen::Vector4d(initial_state->jerk.x, initial_state->jerk.y, initial_state->jerk.z, initial_state->heading_jerk));
+     954             :       }
+     955             : 
+     956         171 :     } else if (i == (waypoints.size() - 1)) {  // the last point
+     957             : 
+     958          16 :       vertex.makeStartOrEnd(Eigen::Vector4d(x, y, z, heading), derivative_to_optimize);
+     959             : 
+     960          16 :       vertex.addConstraint(eth_trajectory_generation::derivative_order::POSITION, Eigen::Vector4d(x, y, z, heading));
+     961             : 
+     962             :     } else {  // mid points
+     963             : 
+     964         155 :       vertex.addConstraint(eth_trajectory_generation::derivative_order::POSITION, Eigen::Vector4d(x, y, z, heading));
+     965             : 
+     966         155 :       if (waypoints.at(i).stop_at) {
+     967           0 :         vertex.addConstraint(eth_trajectory_generation::derivative_order::VELOCITY, Eigen::Vector4d(0, 0, 0, 0));
+     968           0 :         vertex.addConstraint(eth_trajectory_generation::derivative_order::ACCELERATION, Eigen::Vector4d(0, 0, 0, 0));
+     969           0 :         vertex.addConstraint(eth_trajectory_generation::derivative_order::JERK, Eigen::Vector4d(0, 0, 0, 0));
+     970             :       }
+     971             :     }
+     972             : 
+     973         187 :     vertices.push_back(vertex);
+     974             :   }
+     975             : 
+     976             :   // | ---------------- compute the segment times --------------- |
+     977             : 
+     978             :   double v_max_horizontal, a_max_horizontal, j_max_horizontal;
+     979             :   double v_max_vertical, a_max_vertical, j_max_vertical;
+     980             : 
+     981             :   // use the small of the ascending/descending values
+     982          16 :   double vertical_speed_lim        = std::min(constraints->vertical_ascending_speed, constraints->vertical_descending_speed);
+     983          16 :   double vertical_acceleration_lim = std::min(constraints->vertical_ascending_acceleration, constraints->vertical_descending_acceleration);
+     984             : 
+     985          16 :   v_max_horizontal = constraints->horizontal_speed;
+     986          16 :   a_max_horizontal = constraints->horizontal_acceleration;
+     987             : 
+     988          16 :   v_max_vertical = vertical_speed_lim;
+     989          16 :   a_max_vertical = vertical_acceleration_lim;
+     990             : 
+     991          16 :   j_max_horizontal = constraints->horizontal_jerk;
+     992          16 :   j_max_vertical   = std::min(constraints->vertical_ascending_jerk, constraints->vertical_descending_jerk);
+     993             : 
+     994          16 :   if (override_constraints_) {
+     995             : 
+     996           0 :     bool can_change = true;
+     997             : 
+     998             : 
+     999           0 :     if (initial_state) {
+    1000           0 :       can_change = (hypot(initial_state->velocity.x, initial_state->velocity.y) < override_max_velocity_horizontal_) &&
+    1001           0 :                    (hypot(initial_state->acceleration.x, initial_state->acceleration.y) < override_max_acceleration_horizontal_) &&
+    1002           0 :                    (hypot(initial_state->jerk.x, initial_state->jerk.y) < override_max_jerk_horizontal_) &&
+    1003           0 :                    (fabs(initial_state->velocity.z) < override_max_velocity_vertical_) &&
+    1004           0 :                    (fabs(initial_state->acceleration.z) < override_max_acceleration_vertical_) && (fabs(initial_state->jerk.z) < override_max_jerk_vertical_);
+    1005             :     }
+    1006             : 
+    1007           0 :     if (can_change) {
+    1008             : 
+    1009           0 :       v_max_horizontal = override_max_velocity_horizontal_;
+    1010           0 :       a_max_horizontal = override_max_acceleration_horizontal_;
+    1011           0 :       j_max_horizontal = override_max_jerk_horizontal_;
+    1012             : 
+    1013           0 :       v_max_vertical = override_max_velocity_vertical_;
+    1014           0 :       a_max_vertical = override_max_acceleration_vertical_;
+    1015           0 :       j_max_vertical = override_max_jerk_vertical_;
+    1016             : 
+    1017           0 :       ROS_DEBUG("[TrajectoryGeneration]: overriding constraints by a user");
+    1018             : 
+    1019             :     } else {
+    1020             : 
+    1021           0 :       ROS_WARN("[TrajectoryGeneration]: overrifing constraints refused due to possible infeasibility");
+    1022             :     }
+    1023             :   }
+    1024             : 
+    1025             :   double v_max_heading, a_max_heading, j_max_heading;
+    1026             : 
+    1027          16 :   if (relax_heading) {
+    1028           0 :     v_max_heading = std::numeric_limits<float>::max();
+    1029           0 :     a_max_heading = std::numeric_limits<float>::max();
+    1030           0 :     j_max_heading = std::numeric_limits<float>::max();
+    1031             :   } else {
+    1032          16 :     v_max_heading = constraints->heading_speed;
+    1033          16 :     a_max_heading = constraints->heading_acceleration;
+    1034          16 :     j_max_heading = constraints->heading_jerk;
+    1035             :   }
+    1036             : 
+    1037          16 :   ROS_DEBUG("[TrajectoryGeneration]: using constraints:");
+    1038          16 :   ROS_DEBUG("[TrajectoryGeneration]: horizontal: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_horizontal, a_max_horizontal, j_max_horizontal);
+    1039          16 :   ROS_DEBUG("[TrajectoryGeneration]: vertical: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_vertical, a_max_vertical, j_max_vertical);
+    1040          16 :   ROS_DEBUG("[TrajectoryGeneration]: heading: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_heading, a_max_heading, j_max_heading);
+    1041             : 
+    1042          32 :   std::vector<double> segment_times, segment_times_baca;
+    1043          32 :   segment_times      = estimateSegmentTimes(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical,
+    1044          16 :                                        v_max_heading, a_max_heading);
+    1045          32 :   segment_times_baca = estimateSegmentTimesBaca(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical,
+    1046          16 :                                                 v_max_heading, a_max_heading);
+    1047             : 
+    1048          16 :   double initial_total_time      = 0;
+    1049          16 :   double initial_total_time_baca = 0;
+    1050         187 :   for (int i = 0; i < int(segment_times_baca.size()); i++) {
+    1051         171 :     initial_total_time += segment_times.at(i);
+    1052         171 :     initial_total_time_baca += segment_times_baca.at(i);
+    1053             :   }
+    1054             : 
+    1055          16 :   ROS_DEBUG("[TrajectoryGeneration]: initial total time (Euclidean): %.2f", initial_total_time);
+    1056          16 :   ROS_DEBUG("[TrajectoryGeneration]: initial total time (Baca): %.2f", initial_total_time_baca);
+    1057             : 
+    1058             :   // | --------- create an optimizer object and solve it -------- |
+    1059             : 
+    1060          16 :   const int                                                     N = 10;
+    1061          32 :   eth_trajectory_generation::PolynomialOptimizationNonLinear<N> opt(dimension, parameters);
+    1062          16 :   opt.setupFromVertices(vertices, segment_times, derivative_to_optimize);
+    1063             : 
+    1064          16 :   opt.addMaximumMagnitudeConstraint(0, eth_trajectory_generation::derivative_order::VELOCITY, v_max_horizontal);
+    1065          16 :   opt.addMaximumMagnitudeConstraint(0, eth_trajectory_generation::derivative_order::ACCELERATION, a_max_horizontal);
+    1066          16 :   opt.addMaximumMagnitudeConstraint(0, eth_trajectory_generation::derivative_order::JERK, j_max_horizontal);
+    1067             : 
+    1068          16 :   opt.addMaximumMagnitudeConstraint(1, eth_trajectory_generation::derivative_order::VELOCITY, v_max_horizontal);
+    1069          16 :   opt.addMaximumMagnitudeConstraint(1, eth_trajectory_generation::derivative_order::ACCELERATION, a_max_horizontal);
+    1070          16 :   opt.addMaximumMagnitudeConstraint(1, eth_trajectory_generation::derivative_order::JERK, j_max_horizontal);
+    1071             : 
+    1072          16 :   opt.addMaximumMagnitudeConstraint(2, eth_trajectory_generation::derivative_order::VELOCITY, v_max_vertical);
+    1073          16 :   opt.addMaximumMagnitudeConstraint(2, eth_trajectory_generation::derivative_order::ACCELERATION, a_max_vertical);
+    1074          16 :   opt.addMaximumMagnitudeConstraint(2, eth_trajectory_generation::derivative_order::JERK, j_max_vertical);
+    1075             : 
+    1076          16 :   opt.addMaximumMagnitudeConstraint(3, eth_trajectory_generation::derivative_order::VELOCITY, v_max_heading);
+    1077          16 :   opt.addMaximumMagnitudeConstraint(3, eth_trajectory_generation::derivative_order::ACCELERATION, a_max_heading);
+    1078          16 :   opt.addMaximumMagnitudeConstraint(3, eth_trajectory_generation::derivative_order::JERK, j_max_heading);
+    1079             : 
+    1080          16 :   opt.optimize();
+    1081             : 
+    1082          16 :   if (overtime()) {
+    1083           5 :     return {};
+    1084             :   }
+    1085             : 
+    1086          22 :   std::string result_str;
+    1087             : 
+    1088          11 :   switch (opt.getOptimizationInfo().stopping_reason) {
+    1089           4 :     case nlopt::FAILURE: {
+    1090           4 :       result_str = "generic failure";
+    1091           4 :       break;
+    1092             :     }
+    1093           0 :     case nlopt::INVALID_ARGS: {
+    1094           0 :       result_str = "invalid args";
+    1095           0 :       break;
+    1096             :     }
+    1097           0 :     case nlopt::OUT_OF_MEMORY: {
+    1098           0 :       result_str = "out of memory";
+    1099           0 :       break;
+    1100             :     }
+    1101           0 :     case nlopt::ROUNDOFF_LIMITED: {
+    1102           0 :       result_str = "roundoff limited";
+    1103           0 :       break;
+    1104             :     }
+    1105           0 :     case nlopt::FORCED_STOP: {
+    1106           0 :       result_str = "forced stop";
+    1107           0 :       break;
+    1108             :     }
+    1109           0 :     case nlopt::STOPVAL_REACHED: {
+    1110           0 :       result_str = "stopval reached";
+    1111           0 :       break;
+    1112             :     }
+    1113           2 :     case nlopt::FTOL_REACHED: {
+    1114           2 :       result_str = "ftol reached";
+    1115           2 :       break;
+    1116             :     }
+    1117           2 :     case nlopt::XTOL_REACHED: {
+    1118           2 :       result_str = "xtol reached";
+    1119           2 :       break;
+    1120             :     }
+    1121           3 :     case nlopt::MAXEVAL_REACHED: {
+    1122           3 :       result_str = "maxeval reached";
+    1123           3 :       break;
+    1124             :     }
+    1125           0 :     case nlopt::MAXTIME_REACHED: {
+    1126           0 :       result_str = "maxtime reached";
+    1127           0 :       break;
+    1128             :     }
+    1129           0 :     default: {
+    1130           0 :       result_str = "UNKNOWN FAILURE CODE";
+    1131           0 :       break;
+    1132             :     }
+    1133             :   }
+    1134             : 
+    1135          11 :   if (opt.getOptimizationInfo().stopping_reason >= 1 && opt.getOptimizationInfo().stopping_reason != 6) {
+    1136           7 :     ROS_DEBUG("[TrajectoryGeneration]: optimization finished successfully with code %d, '%s'", opt.getOptimizationInfo().stopping_reason, result_str.c_str());
+    1137             : 
+    1138           4 :   } else if (opt.getOptimizationInfo().stopping_reason == -1) {
+    1139           4 :     ROS_DEBUG("[TrajectoryGeneration]: optimization finished with a generic error code %d, '%s'", opt.getOptimizationInfo().stopping_reason,
+    1140             :               result_str.c_str());
+    1141             : 
+    1142             :   } else {
+    1143           0 :     ROS_WARN("[TrajectoryGeneration]: optimization failed with code %d, '%s', took %.3f s", opt.getOptimizationInfo().stopping_reason, result_str.c_str(),
+    1144             :              (ros::Time::now() - find_trajectory_time_start).toSec());
+    1145           0 :     return {};
+    1146             :   }
+    1147             : 
+    1148             :   // | ------------- obtain the polynomial segments ------------- |
+    1149             : 
+    1150          22 :   eth_trajectory_generation::Segment::Vector segments;
+    1151          11 :   opt.getPolynomialOptimizationRef().getSegments(&segments);
+    1152             : 
+    1153          11 :   if (overtime()) {
+    1154           0 :     return {};
+    1155             :   }
+    1156             : 
+    1157             :   // | --------------- create the trajectory class -------------- |
+    1158             : 
+    1159          22 :   eth_trajectory_generation::Trajectory trajectory;
+    1160          11 :   opt.getTrajectory(&trajectory);
+    1161             : 
+    1162          22 :   eth_mav_msgs::EigenTrajectoryPoint::Vector states;
+    1163             : 
+    1164          11 :   ROS_DEBUG("[TrajectoryGeneration]: starting eth sampling with dt = %.2f s ", sampling_dt);
+    1165             : 
+    1166          11 :   bool success = eth_trajectory_generation::sampleWholeTrajectory(trajectory, sampling_dt, &states);
+    1167             : 
+    1168          11 :   if (overtime()) {
+    1169           0 :     return {};
+    1170             :   }
+    1171             : 
+    1172             :   // validate the temporal sampling of the trajectory
+    1173             : 
+    1174             :   // only check this if the trajectory is > 1.0 sec, this check does not make much sense for the short ones
+    1175          11 :   if ((states.size() * sampling_dt) > 1.0 && (states.size() * sampling_dt) > (_max_trajectory_len_factor_ * initial_total_time_baca)) {
+    1176           0 :     ROS_ERROR("[TrajectoryGeneration]: the final trajectory sampling is too long = %.2f, initial 'baca' estimate = %.2f, allowed factor %.2f, aborting",
+    1177             :               (states.size() * sampling_dt), initial_total_time_baca, _max_trajectory_len_factor_);
+    1178             : 
+    1179           0 :     std::stringstream ss;
+    1180           0 :     ss << "trajectory sampling failed";
+    1181           0 :     ROS_ERROR_STREAM("[TrajectoryGeneration]: " << ss.str());
+    1182           0 :     return {};
+    1183             : 
+    1184          11 :   } else if ((states.size() * sampling_dt) > 1.0 && (states.size() * sampling_dt) < (_min_trajectory_len_factor_ * initial_total_time_baca)) {
+    1185           0 :     ROS_ERROR("[TrajectoryGeneration]: the final trajectory sampling is too short = %.2f, initial 'baca' estimate = %.2f, allowed factor %.2f, aborting",
+    1186             :               (states.size() * sampling_dt), initial_total_time_baca, _min_trajectory_len_factor_);
+    1187             : 
+    1188           0 :     std::stringstream ss;
+    1189           0 :     ss << "trajectory sampling failed";
+    1190           0 :     ROS_ERROR_STREAM("[TrajectoryGeneration]: " << ss.str());
+    1191           0 :     return {};
+    1192             : 
+    1193             :   } else {
+    1194          11 :     ROS_DEBUG("[TrajectoryGeneration]: estimated/final trajectory length ratio (final/estimated) %.2f",
+    1195             :               (states.size() * sampling_dt) / initial_total_time_baca);
+    1196             :   }
+    1197             : 
+    1198          11 :   if (success) {
+    1199          11 :     ROS_DEBUG("[TrajectoryGeneration]: eth sampling finished, took %.3f s", (ros::Time::now() - find_trajectory_time_start).toSec());
+    1200          11 :     return std::optional(states);
+    1201             : 
+    1202             :   } else {
+    1203           0 :     ROS_ERROR("[TrajectoryGeneration]: eth could not sample the trajectory, took %.3f s", (ros::Time::now() - find_trajectory_time_start).toSec());
+    1204           0 :     return {};
+    1205             :   }
+    1206             : }
+    1207             : 
+    1208             : //}
+    1209             : 
+    1210             : /* findTrajectoryFallback() //{ */
+    1211             : 
+    1212           6 : std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> MrsTrajectoryGeneration::findTrajectoryFallback(const std::vector<Waypoint_t>& waypoints,
+    1213             :                                                                                                           const double&                  sampling_dt,
+    1214             :                                                                                                           const bool&                    relax_heading) {
+    1215             : 
+    1216          18 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::findTrajectoryFallback", scope_timer_logger_, scope_timer_enabled_);
+    1217             : 
+    1218           6 :   ros::Time time_start = ros::Time::now();
+    1219             : 
+    1220           6 :   ROS_WARN("[TrajectoryGeneration]: fallback sampling started");
+    1221             : 
+    1222          12 :   auto params      = mrs_lib::get_mutexed(mutex_params_, params_);
+    1223          12 :   auto constraints = sh_constraints_.getMsg();
+    1224             : 
+    1225          12 :   eth_trajectory_generation::Vertex::Vector vertices;
+    1226           6 :   const int                                 dimension = 4;
+    1227             : 
+    1228             :   // | --------------- add constraints to vertices -------------- |
+    1229             : 
+    1230           6 :   double last_heading = waypoints.at(0).coords(3);
+    1231             : 
+    1232          36 :   for (size_t i = 0; i < waypoints.size(); i++) {
+    1233             : 
+    1234          30 :     double x       = waypoints.at(i).coords(0);
+    1235          30 :     double y       = waypoints.at(i).coords(1);
+    1236          30 :     double z       = waypoints.at(i).coords(2);
+    1237          30 :     double heading = sradians::unwrap(waypoints.at(i).coords(3), last_heading);
+    1238          30 :     last_heading   = heading;
+    1239             : 
+    1240          60 :     eth_trajectory_generation::Vertex vertex(dimension);
+    1241             : 
+    1242          30 :     vertex.addConstraint(eth_trajectory_generation::derivative_order::POSITION, Eigen::Vector4d(x, y, z, heading));
+    1243             : 
+    1244          30 :     vertices.push_back(vertex);
+    1245             :   }
+    1246             : 
+    1247             :   // | ---------------- compute the segment times --------------- |
+    1248             : 
+    1249             :   double v_max_horizontal, a_max_horizontal, j_max_horizontal;
+    1250             :   double v_max_vertical, a_max_vertical, j_max_vertical;
+    1251             : 
+    1252             :   // use the small of the ascending/descending values
+    1253           6 :   double vertical_speed_lim        = std::min(constraints->vertical_ascending_speed, constraints->vertical_descending_speed);
+    1254           6 :   double vertical_acceleration_lim = std::min(constraints->vertical_ascending_acceleration, constraints->vertical_descending_acceleration);
+    1255             : 
+    1256           6 :   if (override_constraints_) {
+    1257             : 
+    1258           0 :     v_max_horizontal = override_max_velocity_horizontal_;
+    1259           0 :     a_max_horizontal = override_max_acceleration_horizontal_;
+    1260           0 :     j_max_horizontal = override_max_jerk_horizontal_;
+    1261             : 
+    1262           0 :     v_max_vertical = override_max_velocity_vertical_;
+    1263           0 :     a_max_vertical = override_max_acceleration_vertical_;
+    1264           0 :     j_max_vertical = override_max_jerk_vertical_;
+    1265             : 
+    1266           0 :     ROS_DEBUG("[TrajectoryGeneration]: overriding constraints by a user");
+    1267             :   } else {
+    1268             : 
+    1269           6 :     v_max_horizontal = constraints->horizontal_speed;
+    1270           6 :     a_max_horizontal = constraints->horizontal_acceleration;
+    1271             : 
+    1272           6 :     v_max_vertical = vertical_speed_lim;
+    1273           6 :     a_max_vertical = vertical_acceleration_lim;
+    1274             : 
+    1275           6 :     j_max_horizontal = constraints->horizontal_jerk;
+    1276           6 :     j_max_vertical   = std::min(constraints->vertical_ascending_jerk, constraints->vertical_descending_jerk);
+    1277             :   }
+    1278             : 
+    1279             : 
+    1280             :   double v_max_heading, a_max_heading, j_max_heading;
+    1281             : 
+    1282           6 :   if (relax_heading) {
+    1283           0 :     v_max_heading = std::numeric_limits<float>::max();
+    1284           0 :     a_max_heading = std::numeric_limits<float>::max();
+    1285           0 :     j_max_heading = std::numeric_limits<float>::max();
+    1286             :   } else {
+    1287           6 :     v_max_heading = constraints->heading_speed;
+    1288           6 :     a_max_heading = constraints->heading_acceleration;
+    1289           6 :     j_max_heading = constraints->heading_jerk;
+    1290             :   }
+    1291             : 
+    1292           6 :   v_max_horizontal *= _fallback_sampling_speed_factor_;
+    1293           6 :   v_max_vertical *= _fallback_sampling_speed_factor_;
+    1294             : 
+    1295           6 :   a_max_horizontal *= _fallback_sampling_accel_factor_;
+    1296           6 :   a_max_vertical *= _fallback_sampling_accel_factor_;
+    1297             : 
+    1298           6 :   ROS_DEBUG("[TrajectoryGeneration]: using constraints:");
+    1299           6 :   ROS_DEBUG("[TrajectoryGeneration]: horizontal: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_horizontal, a_max_horizontal, j_max_horizontal);
+    1300           6 :   ROS_DEBUG("[TrajectoryGeneration]: vertical: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_vertical, a_max_vertical, j_max_vertical);
+    1301           6 :   ROS_DEBUG("[TrajectoryGeneration]: heading: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_heading, a_max_heading, j_max_heading);
+    1302             : 
+    1303          12 :   std::vector<double> segment_times, segment_times_baca;
+    1304          12 :   segment_times      = estimateSegmentTimes(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical,
+    1305           6 :                                        v_max_heading, a_max_heading);
+    1306          12 :   segment_times_baca = estimateSegmentTimesBaca(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical,
+    1307           6 :                                                 v_max_heading, a_max_heading);
+    1308             : 
+    1309           6 :   double initial_total_time      = 0;
+    1310           6 :   double initial_total_time_baca = 0;
+    1311          30 :   for (int i = 0; i < int(segment_times_baca.size()); i++) {
+    1312          24 :     initial_total_time += segment_times.at(i);
+    1313          24 :     initial_total_time_baca += segment_times_baca.at(i);
+    1314             : 
+    1315          24 :     ROS_DEBUG("[TrajectoryGeneration]: segment time [%d] = %.2f", i, segment_times_baca.at(i));
+    1316             :   }
+    1317             : 
+    1318           6 :   ROS_WARN("[TrajectoryGeneration]: fallback: initial total time (Euclidean): %.2f", initial_total_time);
+    1319           6 :   ROS_WARN("[TrajectoryGeneration]: fallback: initial total time (Baca): %.2f", initial_total_time_baca);
+    1320             : 
+    1321          12 :   eth_mav_msgs::EigenTrajectoryPoint::Vector states;
+    1322             : 
+    1323             :   // interpolate each segment
+    1324          30 :   for (size_t i = 0; i < waypoints.size() - 1; i++) {
+    1325             : 
+    1326          48 :     Eigen::VectorXd start, end;
+    1327             : 
+    1328          24 :     const double segment_time = segment_times_baca.at(i);
+    1329             : 
+    1330             :     int    n_samples;
+    1331             :     double interp_step;
+    1332             : 
+    1333          24 :     if (segment_time > 1e-1) {
+    1334             : 
+    1335          24 :       n_samples = ceil(segment_time / sampling_dt);
+    1336             : 
+    1337             :       // important
+    1338          24 :       if (n_samples > 0) {
+    1339          24 :         interp_step = 1.0 / double(n_samples);
+    1340             :       } else {
+    1341           0 :         interp_step = 0.5;
+    1342             :       }
+    1343             : 
+    1344             :     } else {
+    1345           0 :       n_samples   = 0;
+    1346           0 :       interp_step = 0;
+    1347             :     }
+    1348             : 
+    1349          24 :     ROS_DEBUG("[TrajectoryGeneration]: segment n_samples [%lu] = %d", i, n_samples);
+    1350             : 
+    1351             :     // for the last segment, hit the last waypoint completely
+    1352             :     // otherwise, it is hit as the first sample of the following segment
+    1353          24 :     if (n_samples > 0 && i == waypoints.size() - 2) {
+    1354           6 :       n_samples++;
+    1355             :     }
+    1356             : 
+    1357         987 :     for (int j = 0; j < n_samples; j++) {
+    1358             : 
+    1359         963 :       Waypoint_t point = interpolatePoint(waypoints.at(i), waypoints.at(i + 1), j * interp_step);
+    1360             : 
+    1361         963 :       eth_mav_msgs::EigenTrajectoryPoint eth_point;
+    1362         963 :       eth_point.position_W(0) = point.coords(0);
+    1363         963 :       eth_point.position_W(1) = point.coords(1);
+    1364         963 :       eth_point.position_W(2) = point.coords(2);
+    1365         963 :       eth_point.setFromYaw(point.coords(3));
+    1366             : 
+    1367         963 :       states.push_back(eth_point);
+    1368             : 
+    1369         963 :       if (j == 0 && i > 0 && waypoints.at(i).stop_at) {
+    1370             : 
+    1371           0 :         int insert_samples = int(round(_fallback_sampling_stopping_time_ / sampling_dt));
+    1372             : 
+    1373           0 :         for (int k = 0; k < insert_samples; k++) {
+    1374           0 :           states.push_back(eth_point);
+    1375             :         }
+    1376             :       }
+    1377             :     }
+    1378             :   }
+    1379             : 
+    1380           6 :   bool success = true;
+    1381             : 
+    1382           6 :   ROS_WARN("[TrajectoryGeneration]: fallback: sampling finished, took %.3f s", (ros::Time::now() - time_start).toSec());
+    1383             : 
+    1384             :   // | --------------- create the trajectory class -------------- |
+    1385             : 
+    1386           6 :   if (success) {
+    1387           6 :     return std::optional(states);
+    1388             :   } else {
+    1389           0 :     ROS_ERROR("[TrajectoryGeneration]: fallback: sampling failed");
+    1390           0 :     return {};
+    1391             :   }
+    1392             : }
+    1393             : 
+    1394             : //}
+    1395             : 
+    1396             : /* validateTrajectorySpatial() //{ */
+    1397             : 
+    1398          17 : std::tuple<bool, int, std::vector<bool>, double> MrsTrajectoryGeneration::validateTrajectorySpatial(
+    1399             :     const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory, const std::vector<Waypoint_t>& waypoints) {
+    1400             : 
+    1401          51 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::validateTrajectorySpatial", scope_timer_logger_, scope_timer_enabled_);
+    1402             : 
+    1403             :   // prepare the output
+    1404             : 
+    1405          17 :   std::vector<bool> segments;
+    1406         141 :   for (size_t i = 0; i < waypoints.size() - 1; i++) {
+    1407         124 :     segments.push_back(true);
+    1408             :   }
+    1409             : 
+    1410          17 :   int waypoint_idx = 0;
+    1411             : 
+    1412          17 :   bool   is_safe       = true;
+    1413          17 :   double max_deviation = 0;
+    1414             : 
+    1415        2479 :   for (size_t i = 0; i < trajectory.size() - 1; i++) {
+    1416             : 
+    1417             :     // the trajectory sample
+    1418        2462 :     const vec3_t sample = vec3_t(trajectory.at(i).position_W(0), trajectory.at(i).position_W(1), trajectory.at(i).position_W(2));
+    1419             : 
+    1420             :     // next sample
+    1421        2462 :     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));
+    1422             : 
+    1423             :     // segment start
+    1424        2462 :     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));
+    1425             : 
+    1426             :     // segment end
+    1427             :     const vec3_t segment_end =
+    1428        2462 :         vec3_t(waypoints.at(waypoint_idx + 1).coords(0), waypoints.at(waypoint_idx + 1).coords(1), waypoints.at(waypoint_idx + 1).coords(2));
+    1429             : 
+    1430        2462 :     const double distance_from_segment = distFromSegment(sample, segment_start, segment_end);
+    1431             : 
+    1432        2462 :     const double segment_end_dist = distFromSegment(segment_end, sample, next_sample);
+    1433             : 
+    1434        2462 :     if (waypoint_idx > 0 || max_deviation_first_segment_ || int(waypoints.size()) <= 2) {
+    1435             : 
+    1436        2308 :       if (distance_from_segment > max_deviation) {
+    1437         342 :         max_deviation = distance_from_segment;
+    1438             :       }
+    1439             : 
+    1440        2308 :       if (distance_from_segment > trajectory_max_segment_deviation_) {
+    1441         900 :         segments.at(waypoint_idx) = false;
+    1442         900 :         is_safe                   = false;
+    1443             :       }
+    1444             :     }
+    1445             : 
+    1446        2462 :     if (segment_end_dist < 0.05 && waypoint_idx < (int(waypoints.size()) - 2)) {
+    1447         107 :       waypoint_idx++;
+    1448             :     }
+    1449             :   }
+    1450             : 
+    1451          51 :   return std::tuple(is_safe, trajectory.size(), segments, max_deviation);
+    1452             : }
+    1453             : 
+    1454             : //}
+    1455             : 
+    1456             : // | --------------------- minor routines --------------------- |
+    1457             : 
+    1458             : /* findTrajectoryAsync() //{ */
+    1459             : 
+    1460          16 : std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> MrsTrajectoryGeneration::findTrajectoryAsync(
+    1461             :     const std::vector<Waypoint_t>& waypoints, const std::optional<mrs_msgs::TrackerCommand>& initial_state, const double& sampling_dt,
+    1462             :     const bool& relax_heading) {
+    1463             : 
+    1464          16 :   ROS_DEBUG("[TrajectoryGeneration]: starting the async planning task");
+    1465             : 
+    1466             :   future_trajectory_result_ =
+    1467          16 :       std::async(std::launch::async, &MrsTrajectoryGeneration::findTrajectory, this, waypoints, initial_state, sampling_dt, relax_heading);
+    1468             : 
+    1469        9591 :   while (ros::ok() && future_trajectory_result_.wait_for(std::chrono::milliseconds(1)) != std::future_status::ready) {
+    1470             : 
+    1471        9580 :     if (overtime()) {
+    1472           5 :       ROS_WARN("[TrajectoryGeneration]: async task planning timeout, breaking");
+    1473           5 :       return {};
+    1474             :     }
+    1475             :   }
+    1476             : 
+    1477          11 :   ROS_DEBUG("[TrajectoryGeneration]: async planning task finished successfully");
+    1478             : 
+    1479          11 :   return future_trajectory_result_.get();
+    1480             : }
+    1481             : 
+    1482             : //}
+    1483             : 
+    1484             : /* distFromSegment() //{ */
+    1485             : 
+    1486        4924 : double MrsTrajectoryGeneration::distFromSegment(const vec3_t& point, const vec3_t& seg1, const vec3_t& seg2) {
+    1487             : 
+    1488        4924 :   vec3_t segment_vector = seg2 - seg1;
+    1489        4924 :   double segment_len    = segment_vector.norm();
+    1490             : 
+    1491        4924 :   vec3_t segment_vector_norm = segment_vector;
+    1492        4924 :   segment_vector_norm.normalize();
+    1493             : 
+    1494        4924 :   double point_coordinate = segment_vector_norm.dot(point - seg1);
+    1495             : 
+    1496        4924 :   if (point_coordinate < 0) {
+    1497          25 :     return (point - seg1).norm();
+    1498        4899 :   } else if (point_coordinate > segment_len) {
+    1499        2394 :     return (point - seg2).norm();
+    1500             :   } else {
+    1501             : 
+    1502        2505 :     mat3_t segment_projector = segment_vector_norm * segment_vector_norm.transpose();
+    1503        2505 :     vec3_t projection        = seg1 + segment_projector * (point - seg1);
+    1504             : 
+    1505        2505 :     return (point - projection).norm();
+    1506             :   }
+    1507             : }
+    1508             : 
+    1509             : //}
+    1510             : 
+    1511             : /* getTrajectoryReference() //{ */
+    1512             : 
+    1513           6 : mrs_msgs::TrajectoryReference MrsTrajectoryGeneration::getTrajectoryReference(const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory,
+    1514             :                                                                               const std::optional<mrs_msgs::TrackerCommand>&    initial_condition,
+    1515             :                                                                               const double&                                     sampling_dt) {
+    1516             : 
+    1517           6 :   mrs_msgs::TrajectoryReference msg;
+    1518             : 
+    1519           6 :   if (initial_condition) {
+    1520           6 :     msg.header.stamp = initial_condition->header.stamp;
+    1521             :   } else {
+    1522           0 :     msg.header.stamp = ros::Time::now();
+    1523             :   }
+    1524             : 
+    1525           6 :   msg.header.frame_id = frame_id_;
+    1526           6 :   msg.fly_now         = fly_now_;
+    1527           6 :   msg.loop            = loop_;
+    1528           6 :   msg.use_heading     = use_heading_;
+    1529           6 :   msg.dt              = sampling_dt;
+    1530             : 
+    1531         969 :   for (size_t it = 0; it < trajectory.size(); it++) {
+    1532             : 
+    1533         963 :     mrs_msgs::Reference point;
+    1534             : 
+    1535         963 :     point.position.x = trajectory.at(it).position_W(0);
+    1536         963 :     point.position.y = trajectory.at(it).position_W(1);
+    1537         963 :     point.position.z = trajectory.at(it).position_W(2);
+    1538             : 
+    1539         963 :     if (_override_heading_atan2_ && it < (trajectory.size() - 1)) {
+    1540             : 
+    1541           0 :       const double points_dist = std::hypot(trajectory.at(it + 1).position_W(1) - point.position.y, trajectory.at(it + 1).position_W(0) - point.position.x);
+    1542             : 
+    1543           0 :       if (points_dist < 0.05 && it > 0) {
+    1544             : 
+    1545           0 :         point.heading = msg.points.at(it - 1).heading;
+    1546             : 
+    1547             :       } else {
+    1548           0 :         point.heading = atan2(trajectory.at(it + 1).position_W(1) - point.position.y, trajectory.at(it + 1).position_W(0) - point.position.x);
+    1549             :       }
+    1550             : 
+    1551             :     } else {
+    1552         963 :       point.heading = trajectory.at(it).getYaw();
+    1553             :     }
+    1554             : 
+    1555         963 :     msg.points.push_back(point);
+    1556             :   }
+    1557             : 
+    1558           6 :   return msg;
+    1559             : }
+    1560             : 
+    1561             : //}
+    1562             : 
+    1563             : /* interpolatePoint() //{ */
+    1564             : 
+    1565        1014 : Waypoint_t MrsTrajectoryGeneration::interpolatePoint(const Waypoint_t& a, const Waypoint_t& b, const double& coeff) {
+    1566             : 
+    1567        1014 :   Waypoint_t      out;
+    1568        1014 :   Eigen::Vector4d diff = b.coords - a.coords;
+    1569             : 
+    1570        1014 :   out.coords(0) = a.coords(0) + coeff * diff(0);
+    1571        1014 :   out.coords(1) = a.coords(1) + coeff * diff(1);
+    1572        1014 :   out.coords(2) = a.coords(2) + coeff * diff(2);
+    1573        1014 :   out.coords(3) = radians::interp(a.coords(3), b.coords(3), coeff);
+    1574             : 
+    1575        1014 :   out.stop_at = false;
+    1576             : 
+    1577        2028 :   return out;
+    1578             : }
+    1579             : 
+    1580             : //}
+    1581             : 
+    1582             : /* checkNaN() //{ */
+    1583             : 
+    1584          24 : bool MrsTrajectoryGeneration::checkNaN(const Waypoint_t& a) {
+    1585             : 
+    1586          24 :   if (!std::isfinite(a.coords(0))) {
+    1587           0 :     ROS_ERROR("NaN detected in variable \"a.coords(0)\"!!!");
+    1588           0 :     return false;
+    1589             :   }
+    1590             : 
+    1591          24 :   if (!std::isfinite(a.coords(1))) {
+    1592           0 :     ROS_ERROR("NaN detected in variable \"a.coords(1)\"!!!");
+    1593           0 :     return false;
+    1594             :   }
+    1595             : 
+    1596          24 :   if (!std::isfinite(a.coords(2))) {
+    1597           0 :     ROS_ERROR("NaN detected in variable \"a.coords(2)\"!!!");
+    1598           0 :     return false;
+    1599             :   }
+    1600             : 
+    1601          24 :   if (!std::isfinite(a.coords(3))) {
+    1602           0 :     ROS_ERROR("NaN detected in variable \"a.coords(3)\"!!!");
+    1603           0 :     return false;
+    1604             :   }
+    1605             : 
+    1606          24 :   return true;
+    1607             : }
+    1608             : 
+    1609             : //}
+    1610             : 
+    1611             : /* trajectorySrv() //{ */
+    1612             : 
+    1613           4 : bool MrsTrajectoryGeneration::trajectorySrv(const mrs_msgs::TrajectoryReference& msg) {
+    1614             : 
+    1615           8 :   mrs_msgs::TrajectoryReferenceSrv srv;
+    1616           4 :   srv.request.trajectory = msg;
+    1617             : 
+    1618           4 :   bool res = service_client_trajectory_reference_.call(srv);
+    1619             : 
+    1620           4 :   if (res) {
+    1621             : 
+    1622           4 :     if (!srv.response.success) {
+    1623           0 :       ROS_WARN("[TrajectoryGeneration]: service call for trajectory_reference returned: '%s'", srv.response.message.c_str());
+    1624             :     }
+    1625             : 
+    1626           4 :     return srv.response.success;
+    1627             : 
+    1628             :   } else {
+    1629             : 
+    1630           0 :     ROS_ERROR("[TrajectoryGeneration]: service call for trajectory_reference failed!");
+    1631             : 
+    1632           0 :     return false;
+    1633             :   }
+    1634             : }
+    1635             : 
+    1636             : //}
+    1637             : 
+    1638             : /* transformTrackerCmd() //{ */
+    1639             : 
+    1640           6 : std::optional<mrs_msgs::Path> MrsTrajectoryGeneration::transformPath(const mrs_msgs::Path& path_in, const std::string& target_frame) {
+    1641             : 
+    1642             :   // if we transform to the current control frame, which is in fact the same frame as the tracker_cmd is in
+    1643           6 :   if (target_frame == path_in.header.frame_id) {
+    1644           6 :     return path_in;
+    1645             :   }
+    1646             : 
+    1647             :   // find the transformation
+    1648           0 :   auto tf = transformer_->getTransform(path_in.header.frame_id, target_frame, path_in.header.stamp);
+    1649             : 
+    1650           0 :   if (!tf) {
+    1651           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(),
+    1652             :               path_in.header.stamp.toSec());
+    1653           0 :     return {};
+    1654             :   }
+    1655             : 
+    1656           0 :   mrs_msgs::Path path_out = path_in;
+    1657             : 
+    1658           0 :   path_out.header.stamp    = tf.value().header.stamp;
+    1659           0 :   path_out.header.frame_id = transformer_->frame_to(tf.value());
+    1660             : 
+    1661           0 :   for (size_t i = 0; i < path_in.points.size(); i++) {
+    1662             : 
+    1663           0 :     mrs_msgs::ReferenceStamped waypoint;
+    1664             : 
+    1665           0 :     waypoint.header    = path_in.header;
+    1666           0 :     waypoint.reference = path_in.points.at(i);
+    1667             : 
+    1668           0 :     if (auto ret = transformer_->transform(waypoint, tf.value())) {
+    1669             : 
+    1670           0 :       path_out.points.at(i) = ret.value().reference;
+    1671             : 
+    1672             :     } else {
+    1673           0 :       return {};
+    1674             :     }
+    1675             :   }
+    1676             : 
+    1677           0 :   return path_out;
+    1678             : }
+    1679             : 
+    1680             : //}
+    1681             : 
+    1682             : /* overtime() //{ */
+    1683             : 
+    1684        9635 : bool MrsTrajectoryGeneration::overtime(void) {
+    1685             : 
+    1686        9635 :   auto start_time_total   = mrs_lib::get_mutexed(mutex_start_time_total_, start_time_total_);
+    1687        9635 :   auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_);
+    1688             : 
+    1689        9635 :   double overtime = (ros::Time::now() - start_time_total).toSec();
+    1690             : 
+    1691        9635 :   if (overtime > (OVERTIME_SAFETY_FACTOR * max_execution_time - OVERTIME_SAFETY_OFFSET)) {
+    1692          11 :     return true;
+    1693             :   }
+    1694             : 
+    1695        9624 :   return false;
+    1696             : }
+    1697             : 
+    1698             : //}
+    1699             : 
+    1700             : /* timeLeft() //{ */
+    1701             : 
+    1702          16 : double MrsTrajectoryGeneration::timeLeft(void) {
+    1703             : 
+    1704          16 :   auto start_time_total   = mrs_lib::get_mutexed(mutex_start_time_total_, start_time_total_);
+    1705          16 :   auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_);
+    1706             : 
+    1707          16 :   double current_execution_time = (ros::Time::now() - start_time_total).toSec();
+    1708             : 
+    1709          16 :   if (current_execution_time >= max_execution_time) {
+    1710           0 :     return 0;
+    1711             :   } else {
+    1712          16 :     return max_execution_time - current_execution_time;
+    1713             :   }
+    1714             : }
+    1715             : 
+    1716             : //}
+    1717             : 
+    1718             : // | ------------------------ callbacks ----------------------- |
+    1719             : 
+    1720             : /* callbackPath() //{ */
+    1721             : 
+    1722           1 : void MrsTrajectoryGeneration::callbackPath(const mrs_msgs::Path::ConstPtr msg) {
+    1723             : 
+    1724           1 :   if (!is_initialized_) {
+    1725           0 :     return;
+    1726             :   }
+    1727             : 
+    1728             :   /* preconditions //{ */
+    1729             : 
+    1730           1 :   if (!sh_constraints_.hasMsg()) {
+    1731           0 :     std::stringstream ss;
+    1732           0 :     ss << "missing constraints";
+    1733           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    1734           0 :     return;
+    1735             :   }
+    1736             : 
+    1737           1 :   if (!sh_control_manager_diag_.hasMsg()) {
+    1738           0 :     std::stringstream ss;
+    1739           0 :     ss << "missing control manager diagnostics";
+    1740           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    1741           0 :     return;
+    1742             :   }
+    1743             : 
+    1744           1 :   if (!sh_uav_state_.hasMsg()) {
+    1745           0 :     std::stringstream ss;
+    1746           0 :     ss << "missing UAV state";
+    1747           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    1748           0 :     return;
+    1749             :   }
+    1750             : 
+    1751             :   //}
+    1752             : 
+    1753             :   {
+    1754           2 :     std::scoped_lock lock(mutex_start_time_total_);
+    1755             : 
+    1756           1 :     start_time_total_ = ros::Time::now();
+    1757             :   }
+    1758             : 
+    1759           1 :   double path_time_offset = 0;
+    1760             : 
+    1761           1 :   if (msg->header.stamp != ros::Time(0)) {
+    1762           0 :     path_time_offset = (msg->header.stamp - ros::Time::now()).toSec();
+    1763             :   }
+    1764             : 
+    1765           1 :   if (path_time_offset > 1e-3) {
+    1766             : 
+    1767           0 :     std::scoped_lock lock(mutex_max_execution_time_);
+    1768             : 
+    1769           0 :     max_execution_time_ = std::min(FUTURIZATION_EXEC_TIME_FACTOR * path_time_offset, params_.max_execution_time);
+    1770             : 
+    1771           0 :     ROS_INFO("[TrajectoryGeneration]: setting the max execution time to %.3f s = %.1f * %.3f", max_execution_time_, FUTURIZATION_EXEC_TIME_FACTOR,
+    1772             :              path_time_offset);
+    1773             :   } else {
+    1774             : 
+    1775           1 :     std::scoped_lock lock(mutex_max_execution_time_, mutex_params_);
+    1776             : 
+    1777           1 :     max_execution_time_ = params_.max_execution_time;
+    1778             :   }
+    1779             : 
+    1780           1 :   ROS_INFO("[TrajectoryGeneration]: got path from message");
+    1781             : 
+    1782           1 :   ph_original_path_.publish(msg);
+    1783             : 
+    1784           1 :   if (msg->points.empty()) {
+    1785           0 :     std::stringstream ss;
+    1786           0 :     ss << "received an empty message";
+    1787           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    1788           0 :     return;
+    1789             :   }
+    1790             : 
+    1791           2 :   auto transformed_path = transformPath(*msg, "");
+    1792             : 
+    1793           1 :   if (!transformed_path) {
+    1794           0 :     std::stringstream ss;
+    1795           0 :     ss << "could not transform the path to the current control frame";
+    1796           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    1797           0 :     return;
+    1798             :   }
+    1799             : 
+    1800           1 :   fly_now_                              = transformed_path->fly_now;
+    1801           1 :   use_heading_                          = transformed_path->use_heading;
+    1802           1 :   frame_id_                             = transformed_path->header.frame_id;
+    1803           1 :   override_constraints_                 = transformed_path->override_constraints;
+    1804           1 :   loop_                                 = transformed_path->loop;
+    1805           1 :   override_max_velocity_horizontal_     = transformed_path->override_max_velocity_horizontal;
+    1806           1 :   override_max_velocity_vertical_       = transformed_path->override_max_velocity_vertical;
+    1807           1 :   override_max_acceleration_horizontal_ = transformed_path->override_max_acceleration_horizontal;
+    1808           1 :   override_max_acceleration_vertical_   = transformed_path->override_max_acceleration_vertical;
+    1809           1 :   override_max_jerk_horizontal_         = transformed_path->override_max_jerk_horizontal;
+    1810           1 :   override_max_jerk_vertical_           = transformed_path->override_max_jerk_horizontal;
+    1811           1 :   stop_at_waypoints_                    = transformed_path->stop_at_waypoints;
+    1812             : 
+    1813           1 :   auto params = mrs_lib::get_mutexed(mutex_params_, params_);
+    1814             : 
+    1815           1 :   if (transformed_path->max_execution_time > 0) {
+    1816             : 
+    1817           0 :     std::scoped_lock lock(mutex_max_execution_time_);
+    1818             : 
+    1819           0 :     max_execution_time_ = transformed_path->max_execution_time;
+    1820             : 
+    1821             :   } else {
+    1822             : 
+    1823           1 :     std::scoped_lock lock(mutex_max_execution_time_);
+    1824             : 
+    1825           1 :     max_execution_time_ = params.max_execution_time;
+    1826             :   }
+    1827             : 
+    1828           1 :   if (transformed_path->max_deviation_from_path > 0) {
+    1829           0 :     trajectory_max_segment_deviation_ = transformed_path->max_deviation_from_path;
+    1830             :   } else {
+    1831           1 :     trajectory_max_segment_deviation_ = params.max_deviation;
+    1832             :   }
+    1833             : 
+    1834           1 :   dont_prepend_initial_condition_ = transformed_path->dont_prepend_current_state;
+    1835             : 
+    1836           1 :   std::vector<Waypoint_t> waypoints;
+    1837             : 
+    1838           5 :   for (size_t i = 0; i < transformed_path->points.size(); i++) {
+    1839             : 
+    1840           4 :     double x       = transformed_path->points.at(i).position.x;
+    1841           4 :     double y       = transformed_path->points.at(i).position.y;
+    1842           4 :     double z       = transformed_path->points.at(i).position.z;
+    1843           4 :     double heading = transformed_path->points.at(i).heading;
+    1844             : 
+    1845           4 :     Waypoint_t wp;
+    1846           4 :     wp.coords  = Eigen::Vector4d(x, y, z, heading);
+    1847           4 :     wp.stop_at = stop_at_waypoints_;
+    1848             : 
+    1849           4 :     if (!checkNaN(wp)) {
+    1850           0 :       ROS_ERROR("[TrajectoryGeneration]: NaN detected in waypoint #%d", int(i));
+    1851           0 :       return;
+    1852             :     }
+    1853             : 
+    1854           4 :     waypoints.push_back(wp);
+    1855             :   }
+    1856             : 
+    1857           1 :   if (loop_) {
+    1858           0 :     waypoints.push_back(waypoints.at(0));
+    1859             :   }
+    1860             : 
+    1861           1 :   bool                          success = false;
+    1862           2 :   std::string                   message;
+    1863           2 :   mrs_msgs::TrajectoryReference trajectory;
+    1864             : 
+    1865           2 :   for (int i = 0; i < _n_attempts_; i++) {
+    1866             : 
+    1867             :     // the last iteration and the fallback sampling is enabled
+    1868           2 :     bool fallback_sampling = (_n_attempts_ > 1) && (i == (_n_attempts_ - 1)) && _fallback_sampling_enabled_;
+    1869             : 
+    1870           2 :     std::tie(success, message, trajectory) = optimize(waypoints, transformed_path->header, fallback_sampling, transformed_path->relax_heading);
+    1871             : 
+    1872           2 :     if (success) {
+    1873           1 :       break;
+    1874             :     } else {
+    1875           1 :       if (i < _n_attempts_) {
+    1876           1 :         ROS_WARN("[TrajectoryGeneration]: failed to calculate a feasible trajectory, trying again with different initial conditions!");
+    1877             :       } else {
+    1878           0 :         ROS_WARN("[TrajectoryGeneration]: failed to calculate a feasible trajectory");
+    1879             :       }
+    1880             :     }
+    1881             :   }
+    1882             : 
+    1883           1 :   double total_time = (ros::Time::now() - start_time_total_).toSec();
+    1884             : 
+    1885           1 :   auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_);
+    1886             : 
+    1887           1 :   if (total_time > max_execution_time) {
+    1888           0 :     ROS_ERROR("[TrajectoryGeneration]: trajectory ready, took %.3f s in total (exceeding maxtime %.3f s by %.3f s)", total_time, max_execution_time,
+    1889             :               total_time - max_execution_time);
+    1890             :   } else {
+    1891           1 :     ROS_INFO("[TrajectoryGeneration]: trajectory ready, took %.3f s in total (out of %.3f)", total_time, max_execution_time);
+    1892             :   }
+    1893             : 
+    1894           1 :   trajectory.input_id = transformed_path->input_id;
+    1895             : 
+    1896           1 :   if (success) {
+    1897             : 
+    1898           1 :     bool published = trajectorySrv(trajectory);
+    1899             : 
+    1900           1 :     if (published) {
+    1901             : 
+    1902           1 :       ROS_INFO("[TrajectoryGeneration]: trajectory successfully published");
+    1903             : 
+    1904             :     } else {
+    1905             : 
+    1906           0 :       ROS_ERROR("[TrajectoryGeneration]: could not publish the trajectory");
+    1907             :     }
+    1908             : 
+    1909             :   } else {
+    1910             : 
+    1911           0 :     ROS_ERROR("[TrajectoryGeneration]: failed to calculate a feasible trajectory, no publishing a result");
+    1912             :   }
+    1913             : }
+    1914             : 
+    1915             : //}
+    1916             : 
+    1917             : /* callbackPathSrv() //{ */
+    1918             : 
+    1919           3 : bool MrsTrajectoryGeneration::callbackPathSrv(mrs_msgs::PathSrv::Request& req, mrs_msgs::PathSrv::Response& res) {
+    1920             : 
+    1921           3 :   if (!is_initialized_) {
+    1922           0 :     return false;
+    1923             :   }
+    1924             : 
+    1925             :   /* preconditions //{ */
+    1926             : 
+    1927           3 :   if (!sh_constraints_.hasMsg()) {
+    1928           0 :     std::stringstream ss;
+    1929           0 :     ss << "missing constraints";
+    1930           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    1931             : 
+    1932           0 :     res.message = ss.str();
+    1933           0 :     res.success = false;
+    1934           0 :     return true;
+    1935             :   }
+    1936             : 
+    1937           3 :   if (!sh_control_manager_diag_.hasMsg()) {
+    1938           0 :     std::stringstream ss;
+    1939           0 :     ss << "missing control manager diagnostics";
+    1940           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    1941             : 
+    1942           0 :     res.message = ss.str();
+    1943           0 :     res.success = false;
+    1944           0 :     return true;
+    1945             :   }
+    1946             : 
+    1947           3 :   if (!sh_uav_state_.hasMsg()) {
+    1948           0 :     std::stringstream ss;
+    1949           0 :     ss << "missing UAV state";
+    1950           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    1951             : 
+    1952           0 :     res.message = ss.str();
+    1953           0 :     res.success = false;
+    1954           0 :     return true;
+    1955             :   }
+    1956             : 
+    1957             :   //}
+    1958             : 
+    1959             :   {
+    1960           6 :     std::scoped_lock lock(mutex_start_time_total_);
+    1961             : 
+    1962           3 :     start_time_total_ = ros::Time::now();
+    1963             :   }
+    1964             : 
+    1965           3 :   double path_time_offset = 0;
+    1966             : 
+    1967           3 :   if (req.path.header.stamp != ros::Time(0)) {
+    1968           0 :     path_time_offset = (req.path.header.stamp - ros::Time::now()).toSec();
+    1969             :   }
+    1970             : 
+    1971           3 :   if (path_time_offset > 1e-3) {
+    1972             : 
+    1973           0 :     std::scoped_lock lock(mutex_max_execution_time_);
+    1974             : 
+    1975           0 :     max_execution_time_ = std::min(FUTURIZATION_EXEC_TIME_FACTOR * path_time_offset, params_.max_execution_time);
+    1976             : 
+    1977           0 :     ROS_INFO("[TrajectoryGeneration]: setting the max execution time to %.3f s = %.1f * %.3f", max_execution_time_, FUTURIZATION_EXEC_TIME_FACTOR,
+    1978             :              path_time_offset);
+    1979             :   } else {
+    1980             : 
+    1981           3 :     std::scoped_lock lock(mutex_max_execution_time_, mutex_params_);
+    1982             : 
+    1983           3 :     max_execution_time_ = params_.max_execution_time;
+    1984             :   }
+    1985             : 
+    1986           3 :   ROS_INFO("[TrajectoryGeneration]: got path from service");
+    1987             : 
+    1988           3 :   ph_original_path_.publish(req.path);
+    1989             : 
+    1990           3 :   if (req.path.points.empty()) {
+    1991           0 :     std::stringstream ss;
+    1992           0 :     ss << "received an empty message";
+    1993           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    1994             : 
+    1995           0 :     res.message = ss.str();
+    1996           0 :     res.success = false;
+    1997           0 :     return true;
+    1998             :   }
+    1999             : 
+    2000           9 :   auto transformed_path = transformPath(req.path, "");
+    2001             : 
+    2002           3 :   if (!transformed_path) {
+    2003           0 :     std::stringstream ss;
+    2004           0 :     ss << "could not transform the path to the current control frame";
+    2005           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    2006             : 
+    2007           0 :     res.message = ss.str();
+    2008           0 :     res.success = false;
+    2009           0 :     return true;
+    2010             :   }
+    2011             : 
+    2012           3 :   fly_now_                              = transformed_path->fly_now;
+    2013           3 :   use_heading_                          = transformed_path->use_heading;
+    2014           3 :   frame_id_                             = transformed_path->header.frame_id;
+    2015           3 :   override_constraints_                 = transformed_path->override_constraints;
+    2016           3 :   loop_                                 = transformed_path->loop;
+    2017           3 :   override_max_velocity_horizontal_     = transformed_path->override_max_velocity_horizontal;
+    2018           3 :   override_max_velocity_vertical_       = transformed_path->override_max_velocity_vertical;
+    2019           3 :   override_max_acceleration_horizontal_ = transformed_path->override_max_acceleration_horizontal;
+    2020           3 :   override_max_acceleration_vertical_   = transformed_path->override_max_acceleration_vertical;
+    2021           3 :   override_max_jerk_horizontal_         = transformed_path->override_max_jerk_horizontal;
+    2022           3 :   override_max_jerk_vertical_           = transformed_path->override_max_jerk_horizontal;
+    2023           3 :   stop_at_waypoints_                    = transformed_path->stop_at_waypoints;
+    2024             : 
+    2025           6 :   auto params = mrs_lib::get_mutexed(mutex_params_, params_);
+    2026             : 
+    2027           3 :   if (transformed_path->max_execution_time > 0) {
+    2028             : 
+    2029           0 :     std::scoped_lock lock(mutex_max_execution_time_);
+    2030             : 
+    2031           0 :     max_execution_time_ = transformed_path->max_execution_time;
+    2032             : 
+    2033             :   } else {
+    2034             : 
+    2035           3 :     std::scoped_lock lock(mutex_max_execution_time_);
+    2036             : 
+    2037           3 :     max_execution_time_ = params.max_execution_time;
+    2038             :   }
+    2039             : 
+    2040           3 :   if (transformed_path->max_deviation_from_path > 0) {
+    2041           0 :     trajectory_max_segment_deviation_ = transformed_path->max_deviation_from_path;
+    2042             :   } else {
+    2043           3 :     trajectory_max_segment_deviation_ = params.max_deviation;
+    2044             :   }
+    2045             : 
+    2046           3 :   dont_prepend_initial_condition_ = transformed_path->dont_prepend_current_state;
+    2047             : 
+    2048           6 :   std::vector<Waypoint_t> waypoints;
+    2049             : 
+    2050          15 :   for (size_t i = 0; i < req.path.points.size(); i++) {
+    2051             : 
+    2052          12 :     double x       = transformed_path->points.at(i).position.x;
+    2053          12 :     double y       = transformed_path->points.at(i).position.y;
+    2054          12 :     double z       = transformed_path->points.at(i).position.z;
+    2055          12 :     double heading = transformed_path->points.at(i).heading;
+    2056             : 
+    2057          12 :     Waypoint_t wp;
+    2058          12 :     wp.coords  = Eigen::Vector4d(x, y, z, heading);
+    2059          12 :     wp.stop_at = stop_at_waypoints_;
+    2060             : 
+    2061          12 :     if (!checkNaN(wp)) {
+    2062           0 :       ROS_ERROR("[TrajectoryGeneration]: NaN detected in waypoint #%d", int(i));
+    2063           0 :       res.success = false;
+    2064           0 :       res.message = "invalid path";
+    2065           0 :       return true;
+    2066             :     }
+    2067             : 
+    2068          12 :     waypoints.push_back(wp);
+    2069             :   }
+    2070             : 
+    2071           3 :   if (loop_) {
+    2072           0 :     waypoints.push_back(waypoints.at(0));
+    2073             :   }
+    2074             : 
+    2075           3 :   bool                          success = false;
+    2076           6 :   std::string                   message;
+    2077           3 :   mrs_msgs::TrajectoryReference trajectory;
+    2078             : 
+    2079           5 :   for (int i = 0; i < _n_attempts_; i++) {
+    2080             : 
+    2081             :     // the last iteration and the fallback sampling is enabled
+    2082           5 :     bool fallback_sampling = (_n_attempts_ > 1) && (i == (_n_attempts_ - 1)) && _fallback_sampling_enabled_;
+    2083             : 
+    2084           5 :     std::tie(success, message, trajectory) = optimize(waypoints, transformed_path->header, fallback_sampling, transformed_path->relax_heading);
+    2085             : 
+    2086           5 :     if (success) {
+    2087           3 :       break;
+    2088             :     } else {
+    2089           2 :       if (i < _n_attempts_) {
+    2090           2 :         ROS_WARN("[TrajectoryGeneration]: failed to calculate a feasible trajectory, trying again with different initial conditions!");
+    2091             :       } else {
+    2092           0 :         ROS_WARN("[TrajectoryGeneration]: failed to calculate a feasible trajectory");
+    2093             :       }
+    2094             :     }
+    2095             :   }
+    2096             : 
+    2097           3 :   double total_time = (ros::Time::now() - start_time_total_).toSec();
+    2098             : 
+    2099           3 :   auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_);
+    2100             : 
+    2101           3 :   if (total_time > max_execution_time) {
+    2102           1 :     ROS_ERROR("[TrajectoryGeneration]: trajectory ready, took %.3f s in total (exceeding maxtime %.3f s by %.3f s)", total_time, max_execution_time,
+    2103             :               total_time - max_execution_time);
+    2104             :   } else {
+    2105           2 :     ROS_INFO("[TrajectoryGeneration]: trajectory ready, took %.3f s in total (out of %.3f)", total_time, max_execution_time);
+    2106             :   }
+    2107             : 
+    2108           3 :   trajectory.input_id = transformed_path->input_id;
+    2109             : 
+    2110           3 :   if (success) {
+    2111             : 
+    2112           3 :     bool published = trajectorySrv(trajectory);
+    2113             : 
+    2114           3 :     if (published) {
+    2115             : 
+    2116           3 :       res.success = success;
+    2117           3 :       res.message = message;
+    2118             : 
+    2119             :     } else {
+    2120             : 
+    2121           0 :       std::stringstream ss;
+    2122           0 :       ss << "could not publish the trajectory";
+    2123             : 
+    2124           0 :       res.success = false;
+    2125           0 :       res.message = ss.str();
+    2126             : 
+    2127           0 :       ROS_ERROR_STREAM("[TrajectoryGeneration]: " << ss.str());
+    2128             :     }
+    2129             : 
+    2130             :   } else {
+    2131             : 
+    2132           0 :     ROS_ERROR("[TrajectoryGeneration]: failed to calculate a feasible trajectory, not publishing a result");
+    2133             : 
+    2134           0 :     res.success = success;
+    2135           0 :     res.message = message;
+    2136             :   }
+    2137             : 
+    2138           3 :   return true;
+    2139             : }
+    2140             : 
+    2141             : //}
+    2142             : 
+    2143             : /* callbackGetPathSrv() //{ */
+    2144             : 
+    2145           2 : bool MrsTrajectoryGeneration::callbackGetPathSrv(mrs_msgs::GetPathSrv::Request& req, mrs_msgs::GetPathSrv::Response& res) {
+    2146             : 
+    2147           2 :   if (!is_initialized_) {
+    2148           0 :     return false;
+    2149             :   }
+    2150             : 
+    2151             :   /* preconditions //{ */
+    2152             : 
+    2153           2 :   if (!sh_constraints_.hasMsg()) {
+    2154           0 :     std::stringstream ss;
+    2155           0 :     ss << "missing constraints";
+    2156           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    2157             : 
+    2158           0 :     res.message = ss.str();
+    2159           0 :     res.success = false;
+    2160           0 :     return true;
+    2161             :   }
+    2162             : 
+    2163           2 :   if (!sh_control_manager_diag_.hasMsg()) {
+    2164           0 :     std::stringstream ss;
+    2165           0 :     ss << "missing control manager diagnostics";
+    2166           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    2167             : 
+    2168           0 :     res.message = ss.str();
+    2169           0 :     res.success = false;
+    2170           0 :     return true;
+    2171             :   }
+    2172             : 
+    2173           2 :   if (!sh_uav_state_.hasMsg()) {
+    2174           0 :     std::stringstream ss;
+    2175           0 :     ss << "missing UAV state";
+    2176           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    2177             : 
+    2178           0 :     res.message = ss.str();
+    2179           0 :     res.success = false;
+    2180           0 :     return true;
+    2181             :   }
+    2182             : 
+    2183             :   //}
+    2184             : 
+    2185             :   {
+    2186           4 :     std::scoped_lock lock(mutex_start_time_total_);
+    2187             : 
+    2188           2 :     start_time_total_ = ros::Time::now();
+    2189             :   }
+    2190             : 
+    2191           2 :   double path_time_offset = 0;
+    2192             : 
+    2193           2 :   if (req.path.header.stamp != ros::Time(0)) {
+    2194           0 :     path_time_offset = (req.path.header.stamp - ros::Time::now()).toSec();
+    2195             :   }
+    2196             : 
+    2197           2 :   if (path_time_offset > 1e-3) {
+    2198             : 
+    2199           0 :     std::scoped_lock lock(mutex_max_execution_time_);
+    2200             : 
+    2201           0 :     max_execution_time_ = FUTURIZATION_EXEC_TIME_FACTOR * path_time_offset;
+    2202             : 
+    2203           0 :     ROS_INFO("[TrajectoryGeneration]: setting the max execution time to %.3f s = %.1f * %.3f", max_execution_time_, FUTURIZATION_EXEC_TIME_FACTOR,
+    2204             :              path_time_offset);
+    2205             :   } else {
+    2206             : 
+    2207           2 :     std::scoped_lock lock(mutex_max_execution_time_, mutex_params_);
+    2208             : 
+    2209           2 :     max_execution_time_ = params_.max_execution_time;
+    2210             :   }
+    2211             : 
+    2212           2 :   ROS_INFO("[TrajectoryGeneration]: got path from service");
+    2213             : 
+    2214           2 :   ph_original_path_.publish(req.path);
+    2215             : 
+    2216           2 :   if (req.path.points.empty()) {
+    2217           0 :     std::stringstream ss;
+    2218           0 :     ss << "received an empty message";
+    2219           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    2220             : 
+    2221           0 :     res.message = ss.str();
+    2222           0 :     res.success = false;
+    2223           0 :     return true;
+    2224             :   }
+    2225             : 
+    2226             :   // if the path frame_id is latlon_origin, set UTM zone by calling setLatLon for mrs_lib::transformer using the first point in the trajectory
+    2227           2 :   if (req.path.header.frame_id == "latlon_origin") {
+    2228           0 :     transformer_->setLatLon(req.path.points.front().position.x, req.path.points.front().position.y);
+    2229             :   }
+    2230             : 
+    2231           6 :   auto transformed_path = transformPath(req.path, "");
+    2232             : 
+    2233           2 :   if (!transformed_path) {
+    2234           0 :     std::stringstream ss;
+    2235           0 :     ss << "could not transform the path to the current control frame";
+    2236           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    2237             : 
+    2238           0 :     res.message = ss.str();
+    2239           0 :     res.success = false;
+    2240           0 :     return true;
+    2241             :   }
+    2242             : 
+    2243           2 :   fly_now_                              = transformed_path->fly_now;
+    2244           2 :   use_heading_                          = transformed_path->use_heading;
+    2245           2 :   frame_id_                             = transformed_path->header.frame_id;
+    2246           2 :   override_constraints_                 = transformed_path->override_constraints;
+    2247           2 :   loop_                                 = transformed_path->loop;
+    2248           2 :   override_max_velocity_horizontal_     = transformed_path->override_max_velocity_horizontal;
+    2249           2 :   override_max_velocity_vertical_       = transformed_path->override_max_velocity_vertical;
+    2250           2 :   override_max_acceleration_horizontal_ = transformed_path->override_max_acceleration_horizontal;
+    2251           2 :   override_max_acceleration_vertical_   = transformed_path->override_max_acceleration_vertical;
+    2252           2 :   override_max_jerk_horizontal_         = transformed_path->override_max_jerk_horizontal;
+    2253           2 :   override_max_jerk_vertical_           = transformed_path->override_max_jerk_horizontal;
+    2254           2 :   stop_at_waypoints_                    = transformed_path->stop_at_waypoints;
+    2255             : 
+    2256           4 :   auto params = mrs_lib::get_mutexed(mutex_params_, params_);
+    2257             : 
+    2258           2 :   if (transformed_path->max_execution_time > 0) {
+    2259             : 
+    2260           0 :     std::scoped_lock lock(mutex_max_execution_time_);
+    2261             : 
+    2262           0 :     max_execution_time_ = transformed_path->max_execution_time;
+    2263             : 
+    2264             :   } else {
+    2265             : 
+    2266           2 :     std::scoped_lock lock(mutex_max_execution_time_);
+    2267             : 
+    2268           2 :     max_execution_time_ = params.max_execution_time;
+    2269             :   }
+    2270             : 
+    2271           2 :   if (transformed_path->max_deviation_from_path > 0) {
+    2272           0 :     trajectory_max_segment_deviation_ = transformed_path->max_deviation_from_path;
+    2273             :   } else {
+    2274           2 :     trajectory_max_segment_deviation_ = params.max_deviation;
+    2275             :   }
+    2276             : 
+    2277           2 :   dont_prepend_initial_condition_ = transformed_path->dont_prepend_current_state;
+    2278             : 
+    2279           4 :   std::vector<Waypoint_t> waypoints;
+    2280             : 
+    2281          10 :   for (size_t i = 0; i < transformed_path->points.size(); i++) {
+    2282             : 
+    2283           8 :     double x       = transformed_path->points.at(i).position.x;
+    2284           8 :     double y       = transformed_path->points.at(i).position.y;
+    2285           8 :     double z       = transformed_path->points.at(i).position.z;
+    2286           8 :     double heading = transformed_path->points.at(i).heading;
+    2287             : 
+    2288           8 :     Waypoint_t wp;
+    2289           8 :     wp.coords  = Eigen::Vector4d(x, y, z, heading);
+    2290           8 :     wp.stop_at = stop_at_waypoints_;
+    2291             : 
+    2292           8 :     if (!checkNaN(wp)) {
+    2293           0 :       ROS_ERROR("[TrajectoryGeneration]: NaN detected in waypoint #%d", int(i));
+    2294           0 :       res.success = false;
+    2295           0 :       res.message = "invalid path";
+    2296           0 :       return true;
+    2297             :     }
+    2298             : 
+    2299           8 :     waypoints.push_back(wp);
+    2300             :   }
+    2301             : 
+    2302           2 :   if (loop_) {
+    2303           0 :     waypoints.push_back(waypoints.at(0));
+    2304             :   }
+    2305             : 
+    2306           2 :   bool                          success = false;
+    2307           4 :   std::string                   message;
+    2308           4 :   mrs_msgs::TrajectoryReference trajectory;
+    2309             : 
+    2310           4 :   for (int i = 0; i < _n_attempts_; i++) {
+    2311             : 
+    2312             :     // the last iteration and the fallback sampling is enabled
+    2313           4 :     bool fallback_sampling = (_n_attempts_ > 1) && (i == (_n_attempts_ - 1)) && _fallback_sampling_enabled_;
+    2314             : 
+    2315           4 :     std::tie(success, message, trajectory) = optimize(waypoints, transformed_path->header, fallback_sampling, transformed_path->relax_heading);
+    2316             : 
+    2317           4 :     if (success) {
+    2318           2 :       break;
+    2319             :     } else {
+    2320           2 :       if (i < _n_attempts_) {
+    2321           2 :         ROS_WARN("[TrajectoryGeneration]: failed to calculate a feasible trajectory, trying again with different initial conditions!");
+    2322             :       } else {
+    2323           0 :         ROS_WARN("[TrajectoryGeneration]: failed to calculate a feasible trajectory");
+    2324             :       }
+    2325             :     }
+    2326             :   }
+    2327             : 
+    2328           2 :   double total_time = (ros::Time::now() - start_time_total_).toSec();
+    2329             : 
+    2330           2 :   auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_);
+    2331             : 
+    2332           2 :   if (total_time > max_execution_time) {
+    2333           0 :     ROS_ERROR("[TrajectoryGeneration]: trajectory ready, took %.3f s in total (exceeding maxtime %.3f s by %.3f s)", total_time, max_execution_time,
+    2334             :               total_time - max_execution_time);
+    2335             :   } else {
+    2336           2 :     ROS_INFO("[TrajectoryGeneration]: trajectory ready, took %.3f s in total (out of %.3f)", total_time, max_execution_time);
+    2337             :   }
+    2338             : 
+    2339           2 :   if (success) {
+    2340             : 
+    2341           4 :     std::optional<geometry_msgs::TransformStamped> tf_traj_state = transformer_->getTransform("", req.path.header.frame_id, ros::Time::now());
+    2342             : 
+    2343           2 :     std::stringstream ss;
+    2344             : 
+    2345           2 :     if (!tf_traj_state) {
+    2346           0 :       ss << "could not create TF transformer for the trajectory to the requested frame: \"" << req.path.header.frame_id << "\"";
+    2347           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    2348           0 :       res.success = false;
+    2349           0 :       res.message = ss.str();
+    2350           0 :       return true;
+    2351             :     }
+    2352             : 
+    2353           2 :     trajectory.header.frame_id = transformer_->frame_to(*tf_traj_state);
+    2354             : 
+    2355         328 :     for (unsigned long i = 0; i < trajectory.points.size(); i++) {
+    2356             : 
+    2357         326 :       mrs_msgs::ReferenceStamped trajectory_point;
+    2358         326 :       trajectory_point.header    = trajectory.header;
+    2359         326 :       trajectory_point.reference = trajectory.points.at(i);
+    2360             : 
+    2361         326 :       auto ret = transformer_->transform(trajectory_point, *tf_traj_state);
+    2362             : 
+    2363         326 :       if (!ret) {
+    2364             : 
+    2365           0 :         ss << "trajectory cannot be transformed to the requested frame: \"" << req.path.header.frame_id << "\"";
+    2366           0 :         ROS_WARN_STREAM_THROTTLE(1.0, "[TrajectoryGeneration]: " << ss.str());
+    2367           0 :         res.success = false;
+    2368           0 :         res.message = ss.str();
+    2369           0 :         return true;
+    2370             : 
+    2371             :       } else {
+    2372             : 
+    2373             :         // transform the points in the trajectory to the current frame
+    2374         326 :         trajectory.points.at(i) = ret.value().reference;
+    2375             :       }
+    2376             :     }
+    2377             : 
+    2378           2 :     res.trajectory = trajectory;
+    2379           2 :     res.success    = success;
+    2380           2 :     res.message    = message;
+    2381             : 
+    2382             :   } else {
+    2383             : 
+    2384           0 :     ROS_ERROR("[TrajectoryGeneration]: failed to calculate a feasible trajectory");
+    2385             : 
+    2386           0 :     res.success = success;
+    2387           0 :     res.message = message;
+    2388             :   }
+    2389             : 
+    2390           2 :   return true;
+    2391             : }
+    2392             : 
+    2393             : //}
+    2394             : 
+    2395             : /* callbackUavState() //{ */
+    2396             : 
+    2397      176092 : void MrsTrajectoryGeneration::callbackUavState(const mrs_msgs::UavState::ConstPtr msg) {
+    2398             : 
+    2399      176092 :   if (!is_initialized_) {
+    2400           0 :     return;
+    2401             :   }
+    2402             : 
+    2403      176092 :   ROS_INFO_ONCE("[TrajectoryGeneration]: getting uav state");
+    2404             : 
+    2405      176092 :   transformer_->setDefaultFrame(msg->header.frame_id);
+    2406             : }
+    2407             : 
+    2408             : //}
+    2409             : 
+    2410             : /* //{ callbackDrs() */
+    2411             : 
+    2412         109 : void MrsTrajectoryGeneration::callbackDrs(mrs_uav_trajectory_generation::drsConfig& params, [[maybe_unused]] uint32_t level) {
+    2413             : 
+    2414         109 :   mrs_lib::set_mutexed(mutex_params_, params, params_);
+    2415             : 
+    2416             :   {
+    2417         109 :     std::scoped_lock lock(mutex_max_execution_time_);
+    2418             : 
+    2419         109 :     max_execution_time_ = params.max_execution_time;
+    2420             :   }
+    2421             : 
+    2422         109 :   ROS_INFO("[TrajectoryGeneration]: DRS updated");
+    2423         109 : }
+    2424             : 
+    2425             : //}
+    2426             : 
+    2427             : }  // namespace mrs_uav_trajectory_generation
+    2428             : 
+    2429             : #include <pluginlib/class_list_macros.h>
+    2430         109 : 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..6bff0cc471 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.overview.html @@ -0,0 +1,628 @@ + + + + + + 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 + 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..e3369b7522be812bf74d59e9856e5989cd86387f GIT binary patch literal 7482 zcmV-A9mV2_P)9{2Zu zw_fdy??C#fp-g}kPzYGXYUB|%CE&dy>2tV#hQ|U%1gOVY;Es|u@M!3aW~^lL!rRyC(r=OcpVA;ZYQ1PFf47!f4@9S1x>nQB9ABCW)f^sx`+9b=H&ZX>&F7A8D`4?0#2JO(CvY1~8Uo9O}d zr0;#ZlJVF9@mswb#Up9*DL`Rkc;mf25p})SI``UyF(`kh;TC(%kXFj^F3YjilAh2W zDF~q24O@6r?e;7frzs3}uzANS;a9Xavlq0d1{lD}KTvll0TR#>Ao`~YU>Xw%FcU)# z_&^vVUb^V9fcKjc@Zr1$4d7jc@qW{hzTY@Nb&IPj(zLk5wyD_-OxQ`>hdd(1Y_uC& z?^r<+HQm+RbvpwNBXDQ#!I*=O8s|0U?xZ*&k5l+RiJ*x`=0n))AkMrqNJ$#H!uEe;a!&n4Fl&Rf~i(mCIdXbfy9#5@rafKBqa+!V4Xn<89r-RXSBaEg%$0H#d$`2SoscvF_V^b z#8^|12w25vA?ekn0d-2waaXlo_XV4#5OD&wlz zVgu>Hy>8(Vur$$++b}+|1xc6WF=+ug((r)ae~tL2yLv4Q7$=PC{PKY~j1GpktMR)Q z9v_*#F|IEWV_avF4Ffxxb9*lI`9wd;RR(tKt^&*sx>(wo1(Q@Qv*DB#2#7w;`-?C( zZO)+gard5Sbhf(ehUMkZ#$#Avql0jY$F%3K}P`w&WUoX=q_jL$z|&4Ac2kD)WR zUZ+V_nT25XmFG2@rUIaiN8C75#R4Kuihx9vRvzImFYljN2jF@8`ufh=7Qb8H{~ten z0l=?fyM1kCfCy+%v(%aSZJA}I5Clno;r5hJP{1Z0OH7J9<_^*T9;KG?s3a|8d}#rx z2VfxWrjq}fEyTUiyN!(J z(m?v>d9;j-L5@pD9((v^GhS{xU}W3U>@|nT)Ol}D@a@y5LTB>jj0NPy)70xOIUnV* zPOA=GkQm@hJWwPYVB=W#-67GAz}_niz?}7(2TUth0T7B-V<{FbVBPlMePIP-N0UO# zX$wZ;8#PDbo_Sc2^>evR#lDkx|F^}>h1)Cq-ll?Nu)&l2zV;F9U7YBfm7)iri8OVp zN)LjeuIx{ZG!GQ4r}Hj@6bST*TNg_4bN8#1qTM9p(x3Sw5&wZ{;zDKIIUu z161ys4Wvh$_~6s<>8xv}Kj)gJJ0R@-@;TF(>f~?bO+MrF0A(?T>wD=$F=QmUAH#1= zzzK~xbRb13$CwiQIIyfUVMb5>0G0&=z#c!xBb^QR)T?9c^eIT{mNGkg9R+fh8#niH zlTT4N!fN$<96kxqf-%^d<@MZyaW4+B1>^E^W_bpS`;r~18ZaJAK69`CrvP8MpOOPw zIs@AgZa(Dmg4WuEkwy_<1o#+-_g@BQ`0MaA*F4ru0^T{`whLunRMvJ8)5EB8!@hlwGyBn<90B(dGYSwy>|**4yeY+4mSBGb4@+qk3PcZi6xb!Peva zCPSF{&VL6J{!|c<&n@&TpR+?2w9V%p;F^$n=A6^Djz-;?DQU}I8XW+ zvMWOtg#?(?amAZM;#CbX&1v41yakiM;dP69w9faCHtw(60Q>mXS}}{vK$>@4H}A8$ zS-r1-`t4ru0jAKEaDzU-lnKMFF+>}Kt~UtiuUbtQj35M^!MkCi9hSW-uyuUw!;FzT zSzJAtbi4=wV*p3Os3U#Eq}5khl`TZqLC>@y)d#Nd0A4Drke7KRfc}?Rv&UG}&4kh1nNqy>Yk=$8-{9uC*NgkJCv3ltQ;WkV zYjK9deu8%^#=5TTZU*qE?TprcWI}t%sl$L&;$gp#VN~w9`d3#aflnsk%O`1nP6Vmv zWl}5-s%9|jv%7fIZP2kaxP~O`n%k0W?lAKpet0&?rqUTR;LoOL)f1o>CE$6|)hnWv zj+^f`XInOAtYPB8EMI{Fj(zd`SZqKy(Gf?86BtWKZ{@`&Vfa&P-`q4Bvp1!EY|IzNX)Cz_;fDkNyf*=mpYSLVJ?5+-p8Mn@)o$C;`c7;sAhG~;MSjak5fdrQ(+LYBTsjGsvRjyQK@GXzv)6uRwF)V)P{&8ILf zNaaCE2MpyupeZjl(u<#o0cnWU(x|H%<1-Cd1yGIgNpptiUU!apq-j97Tf=+qD*$xg zk&Ct-ckt{Lrtg?weK#PnE43s<9f!76LFZMJ(rl4e5p&pUOZ(9ojaMO)CSJt}(&k<# z(&_NCfpl?SSSP)Ob^9`v#5EDY4gnzNAROvPCU0S z={|TNF;+|&B&*)(^ryJ2hNMCBjm33^qO z9h^tPsGat?zXvp;!DrD$~?aMMhlOvS@rgHe*ap$iRb|pTL5*c+b8hYf182|Zl*W> z(zAU{kBv<*DtNjj(_CKOqiL=bL1;*7D8*6S5DvP3O{xqrx1n!BK{+nqI|fj;Cc(vv zBjYgkxJ)jb?q@@8OKK=(?6+X#52Y4z>~tDq)@&Fx)61BvVCRw5pvBx7K)R}CMfMbc zfaUJup&3Jrc$P%E``NHNIR`aJ_UdNg@@30*?*pcSKcKDF1F^_)OB#hp3V>RS@r?aAr7lkFDa*O(n4{73|(wTxwqVxga$Gy)w^`ytA)vUP4QWI?*41r#cWP{7lb zL-N`lT0p5Y1h8AFmIL>J(cyK3ez&p`!{e41^_QL3&7!yiLL?~~f*03TQtE&PD*ucd z9qaCZ#;atNw~N4zy>NU~1Z38kAGn-0C$|NW<^Yec^PIl!_da+)3dg=w^*Z9zOZvKl zx5UF2NIDZ{0Ax-U7xWEBdd7g+u}dKra49$+gkhtD1U*veT4&~w767F?J~)gBcONr< z!j%JN2cQ`6XEfYt`4WF#_?Ocl;1Z-~IvNTooA*chE~Oi`6jmwbxGYO}$Sdm%9Y@#0 z{45UiJ%08)c`C&^j31iUFi-CX=Jn_CDBH+?$3Fg>`sj;4X#fe2f*t>e1QwwZ z!4j{2CI=~BsFPC=zqM2t$Zj?#%H(CXv=RLygR*ETl>6|%UR0GP7~Q4t(s}w>wiMvMB>e9WQ-hH?|ne6 zPkF5SVf3**_wC}IxiHzDV%}Af*q>9gc(fjGs7_}>@i+hK2sr7U?)Q+f{D{|cVaz_r zDMju=U~mu53J+g^qZfBOFC4oNJBoG>$5(w{%q5s$VG#+CX>tiBg;FRRUR{BJ<7a*x zE`fIvxY|_^-I-Uq?KcBYDw{XZLLDY$m|Iv)^5r{|HWPxP+qsN#DFV3#ETa~|f2U5_6;n$1$ z-_l6NtbEFDy#l4Wub{N^5m}$u2M3-mc3ow_+!D!|#csmMAVI-ZF~+v~7bu2pvB_O3_8_NCeO#56lMa0NgE#+Y== zMN!Qdk64`U^dve5oq~7i7VBX%OTAW$=ZWPaJo9P_+3Bh^JwtDn((_ZEbl?pPAPcCf zP?7HBR^A4(+{slpW%$~&xR*|6%!*IG}ChU{pz=yj6o zSIx5|l7UbD){IB>XwkjS+Y-LIZL_-dKcEEuHvYL zv~+G}$*7E^>#F9^GlSPD+T3vD?R9YVZl=6F;_ZdRRWi$N-ZaM(((^nP?gGSZl2=@< zQb3=%G!D;f^T)$&iaDcCZ*?bAax9 zVOxeU9+-+|>IP~cct8oGvW1w`5r&%KcIJGVgqtuWKr_agnhgNd^N4e_kvLQ70Mp5t zJ3v^uv$Oc4;UU;BP}udjLAmmeqZrTPNS5ZB-jr{^c+=(}aHNr38eW!EH|wk4m5kFd zr}n7HiaR9VtdJg+Fn-oHrPuq8&OE&MHV5Nj&$4^&L#_b~iK;;@JCrW>`n|+cu^$H{ z5?oThteuf!tq{D8I5Ge6LoW6sql*XJ0ij!{LyNafLh6A4wq5`h5ow?iSIf%Gg_7;3{<%=T`< zsJxyky)>}-DjM;lQOU(KC%M*McYlXLN=#d0c@h7S@pfugBlT%v#Sv$kLRissQgL&y z@;>6V^YF)AME8&PPN3!$KHbYES<>)o1?Um=03PY`d;!iq@0w{jGzP%{Xf^z(!J`3W zC)aw+u3&>UFu4eZeJ*LMeo`p3Fi6XEu@b{V6U$Q@nsp} z&6o1U%FYcKfpBW1VQ0>fc8V&zeYK@@Wi>`P+Hg9y@@Tw=RG&1zSnC71Xk#52G)YY~D37NWh>>?s?-2-b8KaFqWTh?jWOb_U+e z2mvn+kjpnC5v5VugKxzYCYQR`pfhChoGzKjmSIamJ5%7$h3vN!H#_e3(>F=v=gayB z&c(@<%3WFYF-tDwr`XR_e_GPrX~d(pGZdlRYc)oASFr$SUuU-XY=BZ9|G{D6bX-kJ_@FL) z4xYDsXCCNV!lZKHoW)f)80wpqud&&a8tQ;*j6o}UJ`6I;s?df}9>e(k%Vz9*@1+>i z6JI4vS=B?_nV(94_93S9n)~-(O;_$$SVg)pfHZm7Vf^o0i}}AE#w+Criqsc}2LaYG zuGKur|8E|Eve$okWd6|<1ZA)P)W}@f4@HdXHZ=R8nE$|LKXCaESN6k|{Q!P8>*HU} zes~FSr{-h8o=aTE><62M|BWS%PrS%eFaBEMg;I`xZLF`Hk1OW`&%!I`AvPk&FEr{pJBJnt{EE0(E$|7-d z=KIPb2?GDUEE4gp+Lw<^9WP>~SIH>WJgMQzA}Ir^F@8`M311br5l}~ZXu3%;%yj^Q zFjvS#$rGw{>p%6))PMyh8w)0(t=x)8{IMh(y?B7U`v6`wQ;Qf=J&;ZnJTw_YyJ@;I zhRW^A7^-9ZC-PE6#k?|xDx|NBp$h50Gcs4kP!XfLT^U0km@$-oZZQB5(f5@xlmM@c zp(|sE1^lfgZt|7ZL##YS(5Fz7ecxl{&&BbB$)0@>YjCtH{ptJrN`In&HnT@Y;YxoJ z0bf`8)0O_D#(1SaRcz0d{`6VtPoBb6^5*!}EB#3_q*wY=)$CpAPx&-(fXa5IKUFba z=}&IXd|&BLLEu*roixdQCjDtdvJ%^p;W=8H?J1tITQH^wQVH-2#c_Nz=J>(uhW{q_ zQqTrqb<_KMRDRFD;ibq9tZ8HxKLOy<8E3_u0M!@?laMhPZ~=aMzITIDd&PAL^2>q5)yKDU+!?- z{L^0Un<+SQ&nH$$I2Gs+s@8>De-|Kf=vB9%W^h&&T`o1Cu3fo>RSj2;_3KaO7WR4^ zOwKLDpO4Bd{KY2;nEuW$Jbh0cOLJE5W7I6D*az6MgRZ+07*qoM6N<$ Ef;Mn-y#N3J 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